Work on adding advantagekit

.gitattributes

@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto

.github/workflows/gradle.yml

@@ -0,0 +1,23 @@
+name: Java CI
+
+on:
+  push:
+    branches:
+    - master
+  pull_request:
+    branches:
+    - master
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v1
+    - name: Set up JDK 17
+      uses: actions/setup-java@v1
+      with:
+        java-version: 17
+    - name: Change wrapper permissions
+      run: chmod +x ./gradlew
+    - name: Build with Gradle
+      run: ./gradlew build

.gitignore

@@ -182,3 +182,6 @@ ctre_sim/
 # clangd
 /.cache
 compile_commands.json
+
+# Eclipse generated file for annotation processors
+.factorypath

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Main",
+      "request": "launch",
+      "mainClass": "frc4388.robot.Main",
+      "projectName": "2025RidgeScape"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

.vscode/settings.json

@@ -25,5 +25,38 @@
       }
     },
   ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "java.import.gradle.annotationProcessing.enabled": false,
+  "java.completion.favoriteStaticMembers": [
+    "org.junit.Assert.*",
+    "org.junit.Assume.*",
+    "org.junit.jupiter.api.Assertions.*",
+    "org.junit.jupiter.api.Assumptions.*",
+    "org.junit.jupiter.api.DynamicContainer.*",
+    "org.junit.jupiter.api.DynamicTest.*",
+    "org.mockito.Mockito.*",
+    "org.mockito.ArgumentMatchers.*",
+    "org.mockito.Answers.*",
+    "edu.wpi.first.units.Units.*"
+  ],
+  "java.completion.filteredTypes": [
+    "java.awt.*",
+    "com.sun.*",
+    "sun.*",
+    "jdk.*",
+    "org.graalvm.*",
+    "io.micrometer.shaded.*",
+    "java.beans.*",
+    "java.util.Base64.*",
+    "java.util.Timer",
+    "java.sql.*",
+    "javax.swing.*",
+    "javax.management.*",
+    "javax.smartcardio.*",
+    "edu.wpi.first.math.proto.*",
+    "edu.wpi.first.math.**.proto.*",
+    "edu.wpi.first.math.**.struct.*",
+  ],
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable",
+  "wpilib.autoStartRioLog": false
 }

.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2025beta",
+    "projectYear": "2025",
     "teamNumber": 4388
 }

README.md

@@ -0,0 +1,2 @@
+# Robot-Essentials
+ Basic code for any Ridgebotics robot project

build.gradle

@@ -1,6 +1,7 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
+    id "com.peterabeles.gversion" version "1.10"
 }
 
 java {
@@ -34,7 +35,7 @@ deploy {
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
                     deleteOldFiles = false // Change to true to delete files on roboRIO that no
-                                           // longer exist in deploy directory on roboRIO
+                                           // longer exist in deploy directory of this project
                 }
             }
         }
@@ -72,6 +73,9 @@ 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 {
@@ -102,3 +106,19 @@ wpi.java.configureTestTasks(test)
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
 }
+
+task(replayWatch, type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.ReplayWatch"
+    classpath = sourceSets.main.runtimeClasspath
+}
+
+// Create version file
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+    srcDir       = "src/main/java/"
+    classPackage = "frc4388.robot.constants"
+    className    = "BuildConstants"
+    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+    timeZone     = "America/Denver"
+    indent       = "  "
+}

simgui-ds.json

@@ -0,0 +1,112 @@
+{
+  "System Joysticks": {
+    "window": {
+      "enabled": false
+    }
+  },
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "030000005e040000ea0200000b050000",
+      "useGamepad": true
+    },
+    {
+      "useGamepad": true
+    },
+    {},
+    {},
+    {},
+    {
+      "useGamepad": true
+    }
+  ]
+}

src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto

@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "lower-algae-removal"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "1 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto

@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "lower-algae-removal"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "1 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 1 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 5 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Feed to Reef 6"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 2 to Reef 5"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 5 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Feed to Reef 6"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 3 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 5 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Feed to Reef 6"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 4 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 3 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 5 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 3 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 6 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 3 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 4 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Feed to Reef 6"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 4 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto

@@ -0,0 +1,109 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 2 to Reef 5"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 5 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Feed to Reef 6"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Feed to Reef 6"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto

@@ -0,0 +1,121 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 5 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 3 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto

@@ -0,0 +1,109 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 4 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/New Auto.auto

@@ -7,7 +7,49 @@
         {
           "type": "path",
           "data": {
-            "pathName": "New Path"
+            "pathName": "Cage 1 to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 2 to Reef 5"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 3 to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 4 to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 5 to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 6 to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Left"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Right"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Taxi.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "taxi"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/test.auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "test"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3040625,
+        "y": 1.3559375
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.892340424329296,
+        "y": 2.067688275398428
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.5221875,
+        "y": 2.3675
+      },
+      "prevControl": {
+        "x": 2.589485733053583,
+        "y": 1.921600370835535
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 59.28109573597083
+  },
+  "reversed": false,
+  "folder": "Bottom Feed to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 53.426969021480645
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Cage 1 to Reef.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.9834375,
+        "y": 7.1084375
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.292598684210526,
+        "y": 6.699835526315789
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.590213815789473,
+        "y": 5.622203947368421
+      },
+      "prevControl": {
+        "x": 5.846151315789473,
+        "y": 6.024391447368421
+      },
+      "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": -118.81079374297312
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -115.9743939624313
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.0078125,
+        "y": 6.0725
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.902067334527623,
+        "y": 5.78751784036735
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.590213815789473,
+        "y": 5.593338815789473
+      },
+      "prevControl": {
+        "x": 5.872115174798128,
+        "y": 6.170888760790882
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 5.15,
+        "y": 4.85
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -120.52970589993409
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -123.4533094540727
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Cage 3 to Reef.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.9590625,
+        "y": 4.9878124999999995
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.709257689548163,
+        "y": 4.997689578268469
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.619078947368421,
+        "y": 5.593338815789473
+      },
+      "prevControl": {
+        "x": 6.094391447368421,
+        "y": 5.5202138157894725
+      },
+      "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": -121.26879518614867
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -124.91940201245771
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Cage 4 to Reef.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.97125,
+        "y": 3.1474999999999995
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.166875,
+        "y": 2.8184375
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.55172697368421,
+        "y": 2.4470394736842103
+      },
+      "prevControl": {
+        "x": 6.3804769736842095,
+        "y": 2.7395394736842107
+      },
+      "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": 120.96375653207355
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 118.9091836511478
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Cage 5 to Reef.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.016603535353535,
+        "y": 1.9718434343434341
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.670197826108406,
+        "y": 2.0396270358261894
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.5805921052631575,
+        "y": 2.4374177631578946
+      },
+      "prevControl": {
+        "x": 5.9030178398827875,
+        "y": 2.377301864673819
+      },
+      "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": 1.0,
+    "rotation": 119.99999999999999
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 123.5866498700801
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Cage 6 to Reef.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.057260101010102,
+        "y": 0.8537878787878788
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.315277777777778,
+        "y": 1.5144570707070704
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.561348684210526,
+        "y": 2.456661184210526
+      },
+      "prevControl": {
+        "x": 5.883902345826687,
+        "y": 2.038842371079213
+      },
+      "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": 120.98817835541992
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 132.5633517531898
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.1296875,
+        "y": 4.0493749999999995
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.833510101010102,
+        "y": 4.06893308080808
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.3496875,
+        "y": 3.8421874999999996
+      },
+      "prevControl": {
+        "x": 6.599301094934041,
+        "y": 3.8282931405651617
+      },
+      "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": -179.96096735022735
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -179.2890804030095
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.1053125,
+        "y": 4.061562499999999
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.809135101010102,
+        "y": 4.081120580808079
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.3496875,
+        "y": 4.195625
+      },
+      "prevControl": {
+        "x": 6.599301094934041,
+        "y": 4.181730640565162
+      },
+      "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": -179.96096735022735
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -179.2890804030095
+  },
+  "useDefaultConstraints": true
+}

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

@@ -3,45 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 13.16336824764045,
-        "y": 1.4493814298363055
+        "x": 4.0,
+        "y": 6.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.079377311296996,
-        "y": 1.379811121204163
+        "x": 4.2495147804709745,
+        "y": 5.984431624152738
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 13.16336824764045,
-        "y": 2.6204816251440413
+        "x": 4.0,
+        "y": 6.0
       },
       "prevControl": {
-        "x": 14.160542671367828,
-        "y": 2.550911316511899
-      },
-      "nextControl": {
-        "x": 12.223879630793721,
-        "y": 2.686027342598464
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 13.16336824764045,
-        "y": 1.4493814298363055
-      },
-      "prevControl": {
-        "x": 12.200978978229143,
-        "y": 1.518951738468449
+        "x": 4.249520880858856,
+        "y": 5.984529705386749
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "mid"
+      "linkedName": null
     }
   ],
   "rotationTargets": [],

src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5221875,
+        "y": 2.4040625
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.9564891755960816,
+        "y": 2.1114702488693777
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3284375,
+        "y": 1.3803125
+      },
+      "prevControl": {
+        "x": 2.0502196323692035,
+        "y": 1.7292616486680126
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.5,
+    "maxAcceleration": 5.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 55.37584492005105
+  },
+  "reversed": false,
+  "folder": "Reef to Bottom Feed",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 58.10920819815426
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.426644736842105,
+        "y": 2.851151315789474
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.23151774045974,
+        "y": 2.4081021765718877
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.7415296052631577,
+        "y": 1.5618421052631584
+      },
+      "prevControl": {
+        "x": 3.369389020277641,
+        "y": 1.8741228567589594
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 55.05578949900953
+  },
+  "reversed": false,
+  "folder": "Reef to Bottom Feed",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 121.42956561483854
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.9750822368421055,
+        "y": 3.9865131578947364
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.0046052631578934,
+        "y": 1.6388157894736843
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3040625,
+        "y": 1.368125
+      },
+      "prevControl": {
+        "x": 1.7992598684210526,
+        "y": 2.302713815789474
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 52.857102599919905
+  },
+  "reversed": false,
+  "folder": "Reef to Bottom Feed",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -179.46454101443553
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.946217105263157,
+        "y": 4.178947368421053
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.604342105263157,
+        "y": 6.518947368421053
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.452878289473684,
+        "y": 6.565131578947369
+      },
+      "prevControl": {
+        "x": 2.0499142743221688,
+        "y": 5.561413144603935
+      },
+      "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": -53.93780932479242
+  },
+  "reversed": false,
+  "folder": "Reef to Top Feed",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 178.9390883097358
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.41702302631579,
+        "y": 5.256578947368421
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.4044191919191915,
+        "y": 5.9460227272727275
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.5076388888888888,
+        "y": 6.484722222222222
+      },
+      "prevControl": {
+        "x": 3.184818220656944,
+        "y": 6.004734862812045
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.5,
+    "maxAcceleration": 5.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -54.83470564502973
+  },
+  "reversed": false,
+  "folder": "Reef to Top Feed",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -121.0370223454415
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5099747474747476,
+        "y": 5.742739898989899
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.36577546257746,
+        "y": 6.2120596924932325
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4143914473684207,
+        "y": 6.584375
+      },
+      "prevControl": {
+        "x": 2.7066049155713703,
+        "y": 6.056626627340928
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.5,
+    "maxAcceleration": 5.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.93780932479242
+  },
+  "reversed": false,
+  "folder": "Reef to Top Feed",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -58.96173664449721
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.31625,
+        "y": 6.730625
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.5451175614850996,
+        "y": 6.277541643530879
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.540467171717172,
+        "y": 5.7630681818181815
+      },
+      "prevControl": {
+        "x": 2.590802656557517,
+        "y": 6.105344244914677
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.239047023115106
+  },
+  "reversed": false,
+  "folder": "Top Feed to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -52.93323259086456
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/test.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.1457991803278684,
+        "y": 5.193801229508197
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.1457991803278684,
+        "y": 5.193801229508197
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.1457991803278684,
+        "y": 5.745235655737704
+      },
+      "prevControl": {
+        "x": 3.8039959016393414,
+        "y": 5.682274590163933
+      },
+      "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

@@ -2,18 +2,28 @@
   "robotWidth": 0.9,
   "robotLength": 0.9,
   "holonomicMode": false,
-  "pathFolders": [],
-  "autoFolders": [],
+  "pathFolders": [
+    "Barge to Reef",
+    "Bottom Feed to Reef",
+    "Reef to Top Feed",
+    "Reef to Bottom Feed",
+    "Top Feed to Reef"
+  ],
+  "autoFolders": [
+    "1 Coral",
+    "2 Coral",
+    "3 Coral"
+  ],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 74.088,
+  "robotMass": 58.18,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
-  "driveGearing": 5.143,
+  "driveWheelRadius": 0.0508,
+  "driveGearing": 6.12,
   "maxDriveSpeed": 5.45,
   "driveMotorType": "krakenX60",
   "driveCurrentLimit": 60.0,

src/main/java/frc4388/robot/Constants.java

@@ -1,200 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot;
-
-import com.pathplanner.lib.config.ModuleConfig;
-import com.pathplanner.lib.config.RobotConfig;
-import com.pathplanner.lib.controllers.PPLTVController;
-import com.pathplanner.lib.controllers.PathFollowingController;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.system.plant.DCMotor;
-
-// import com.pathplanner.l;
-
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import frc4388.utility.Gains;
-import frc4388.utility.LEDPatterns;
-
-/**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
- *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
- * constants are needed, to reduce verbosity.
- */
-public final class Constants {
-    public static final class SwerveDriveConstants {
-
-        public static final double MAX_ROT_SPEED        = 3.5;
-        public static final double AUTO_MAX_ROT_SPEED = 1.5;
-        public static final double MIN_ROT_SPEED        = 1.0;
-        public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
-        public static       double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
-        public static       double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
-
-        public static final String CANBUS_NAME = "IDK";
-    
-        public static final double CORRECTION_MIN = 10;
-        public static final double CORRECTION_MAX = 50;
-        
-        public static final double[] GEARS = {0.25, 0.5, 1.0};
-
-        public static final double SLOW_SPEED = 0.25;
-        public static final double FAST_SPEED = 0.5;
-        public static final double TURBO_SPEED = 1.0;
-    
-        public static final class DefaultSwerveRotOffsets {
-          public static final double FRONT_LEFT_ROT_OFFSET  = 0.0; //TODO: per robot swerve module offsets.
-          public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
-          public static final double BACK_LEFT_ROT_OFFSET   = 0.0; //TODO: per robot swerve module offsets.
-          public static final double BACK_RIGHT_ROT_OFFSET  = 0.0; //TODO: per robot swerve module offsets.
-        }
-
-        public static final class IDs {
-            public static final int FR_ID = 22;
-            public static final int FL_ID = 20;
-            public static final int BL_ID = 21;
-            public static final int BR_ID = 23;
-            // public static final int RIGHT_FRONT_WHEEL_ID = 2;
-            // public static final int RIGHT_FRONT_STEER_ID = 3;
-            // public static final int RIGHT_FRONT_ENCODER_ID = 10;
-            
-            // public static final int LEFT_FRONT_WHEEL_ID = 4;
-            // public static final int LEFT_FRONT_STEER_ID = 5;
-            // public static final int LEFT_FRONT_ENCODER_ID = 11;
-            
-            // public static final int LEFT_BACK_WHEEL_ID = 6;
-            // public static final int LEFT_BACK_STEER_ID = 7;
-            // public static final int LEFT_BACK_ENCODER_ID = 12;
-            
-            // public static final int RIGHT_BACK_WHEEL_ID = 8;  
-            // public static final int RIGHT_BACK_STEER_ID = 9;
-            // public static final int RIGHT_BACK_ENCODER_ID = 13;
-
-            public static final int DRIVE_PIGEON_ID = 10;
-        }
-    
-        public static final class PIDConstants {
-            public static final int SWERVE_SLOT_IDX = 0;
-            public static final int SWERVE_PID_LOOP_IDX = 1;
-            public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0);
-
-            public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
-
-        }
-    
-        public static final class AutoConstants {
-            public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
-            public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
-            public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
-            public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
-            
-            public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
-            public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
-
-            public static final double MASS = 10; // KG
-            public static final double MOI = 10;  // Moment of inertia
-
-            public static final double WHEEL_RADIUS = 0.05; // Meters
-            public static final double MAX_DRIVE_VELOCITY = 0.5; // Meters per second
-            public static final double MAX_ROT_SPEED = 0.2; // Rotations per second
-
-
-            public static final double COEFFICENT_OF_FRICTION = 1.0; // Between 0 and 1
-            public static final DCMotor TALON_SRX_MOTOR = DCMotor.getVex775Pro(1); //TODO: Get actual motor constants
-            public static final double DRIVE_CURRENT_LIMIT = 100000; //TODO: Get actual value
-
-
-            public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(
-                WHEEL_RADIUS, 
-                MAX_DRIVE_VELOCITY, 
-                COEFFICENT_OF_FRICTION, 
-                TALON_SRX_MOTOR,
-                DRIVE_CURRENT_LIMIT, 
-                2);
- 
-            public static final RobotConfig PP_ROBOT_CONFIG = new RobotConfig(
-                MASS, 
-                MOI, 
-                MODULE_CONFIG,
-                new Translation2d[] {
-                    new Translation2d(),
-                    new Translation2d(),
-                    new Translation2d(),
-                    new Translation2d(),
-                });
-
-                
-            public static final double ROBOT_LOOP_TIME = 0.02; // Time it takes for the robot to run a loop
-            public static final PathFollowingController PP_PATH_FOLLOWING_CONTROLLER = new PPLTVController(ROBOT_LOOP_TIME);
-        }
-    
-        public static final class Conversions {
-            public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22;
-            public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5;
-        
-            public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
-            public static final double MOTOR_REV_PER_STEER_REV = 12.8;
-        
-            public static final double TICKS_PER_MOTOR_REV = 0.5;
-            public static final double WHEEL_DIAMETER_INCHES = 3.9;
-            public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
-        
-            public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
-            public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
-            public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
-            public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
-        
-            public static final double TICK_TIME_TO_SECONDS = 10;
-            public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
-        }
-    
-        public static final class Configurations {
-            public static final double OPEN_LOOP_RAMP_RATE = 0.2;
-            public static final double CLOSED_LOOP_RAMP_RATE = 0.2;
-            public static final double NEUTRAL_DEADBAND = 0.04;
-        }
-    
-        public static final double MAX_SPEED_FEET_PER_SECOND = 20.4;
-        public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
-    
-        // dimensions
-        public static final double WIDTH = 18.5;
-        public static final double HEIGHT = 18.5;
-        public static final double HALF_WIDTH = WIDTH / 2.d;
-        public static final double HALF_HEIGHT = HEIGHT / 2.d;
-    
-        // misc
-        public static final int TIMEOUT_MS = 30;
-        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
-      }
-    
-    public static final class VisionConstants {
-        public static final String CAMERA_NAME = "Camera_Module_v1";
-    }
-
-    public static final class DriveConstants {
-        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
-    }
-    
-    public static final class LEDConstants {
-        public static final int LED_SPARK_ID = 0;
-
-        public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
-    }
-
-    public static final class OIConstants {
-        public static final int XBOX_DRIVER_ID = 0;
-        public static final int XBOX_OPERATOR_ID = 1;
-        public static final int XBOX_PROGRAMMER_ID = 2;
-        public static final double LEFT_AXIS_DEADBAND = 0.20;
-
-    }
-}

src/main/java/frc4388/robot/Robot.java

@@ -7,14 +7,24 @@
 
 package frc4388.robot;
 
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+// Advantagekit
+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.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.robot.constants.BuildConstants;
+import frc4388.robot.constants.Constants.SimConstants;
 import frc4388.utility.DeferredBlock;
-import frc4388.utility.RobotTime;
+import frc4388.utility.compute.RobotTime;
+import frc4388.utility.compute.Trim;
+import frc4388.utility.status.FaultReporter;
+
 //import frc4388.robot.subsystems.LED;
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -23,7 +33,7 @@ import frc4388.utility.RobotTime;
  * creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
   Command m_autonomousCommand;
 
   private RobotTime m_robotTime = RobotTime.getInstance();
@@ -35,12 +45,20 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotInit() {
+    // Start logging with AdvantageKit
+    startLogging();
 
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
+
+    // // Create a shuffleboard update thread, that will periodically update the values on shuffleboard
+    // FaultReporter.startThread();
   }
 
+
+
   /**
    * This function is called every robot packet, no matter the mode. Use
    * this for items like diagnostics that you want ran during disabled,
@@ -52,15 +70,15 @@ public class Robot extends TimedRobot {
   @Override
   public void robotPeriodic() {   
     m_robotTime.updateTimes();
-    //System.out.println(m_robotContainer.limelight.isNearSpeaker());
-    //mled.updateLED();
+    // SmartDashboard.putNumber("Time", System.currentTimeMillis());
+    
+    // m_robotContainer.m_robotLED.update();
     // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
   }
-
   /**
    * This function is called once each time the robot enters Disabled mode.
    * You can use it to reset any subsystem information you want to clear when
@@ -108,14 +126,20 @@ public class Robot extends TimedRobot {
   public void autonomousPeriodic() {
   }
 
+
   @Override
   public void teleopInit() {
+    m_robotContainer.stop();
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
+
     if (m_autonomousCommand != null) {
+      CommandScheduler.getInstance().cancel(m_autonomousCommand);
       m_autonomousCommand.cancel();
+      m_autonomousCommand.end(true);
+
     }
     m_robotTime.startMatchTime();
   }
@@ -129,9 +153,112 @@ public class Robot extends TimedRobot {
   }
 
   /**
-   * This function is called periodically during test mode.
+   * This function is called periodically during operator control.
    */
   @Override
-  public void testPeriodic() {
+  public void teleopExit() { // the only OTHER mode that teleop can enter into is disabled.
+    Trim.dumpAll();
+  }
+
+  @Override
+  public void testInit() {
+    FaultReporter.printReport();
+  }
+
+
+
+
+
+
+
+  // VisionSystemSim visionSim;
+  // RobotMapSim robotMapSim;
+
+  // @Override
+  // public void simulationInit() {
+  //   visionSim = new VisionSystemSim("main");
+  //   robotMapSim = m_robotContainer.m_robotMap.configureSim();
+
+
+  //   // A 0.5 x 0.25 meter rectangular target
+  //   TargetModel targetModel = new TargetModel(0.5, 0.25);
+  //   // The pose of where the target is on the field.
+  //   // Its rotation determines where "forward" or the target x-axis points.
+  //   // Let's say this target is flat against the far wall center, facing the blue driver stations.
+  //   Pose3d targetPose = new Pose3d(16, 4, 2, new Rotation3d(0, 0, Math.PI));
+  //   // The given target model at the given pose
+  //   VisionTargetSim visionTarget = new VisionTargetSim(targetPose, targetModel);
+
+  //   // Add this vision target to the vision system simulation to make it visible
+  //   visionSim.addVisionTargets(visionTarget);
+
+  //   // The layout of AprilTags which we want to add to the vision system
+  //   AprilTagFieldLayout tagLayout = Constants.FieldConstants.kTagLayout;
+
+  //   visionSim.addAprilTags(tagLayout);
+
+
+  //   visionSim.addCamera(robotMapSim.leftCamera, Constants.VisionConstants.LEFT_CAMERA_POS);
+  //   visionSim.addCamera(robotMapSim.rightCamera, Constants.VisionConstants.RIGHT_CAMERA_POS);
+
+  //   SmartDashboard.putData(visionSim.getDebugField());
+  // }
+
+
+  @Override
+  public void simulationPeriodic() {
+    // m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage());
+    // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d());
+
+    // m_robotContainer.m_robotSwerveDrive.
+  }
+
+
+
+// Start AdvantageKit logging / replay and record metadata
+// Refrence: https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/template_projects/sources/skeleton/src/main/java/frc/robot/Robot.java
+  public void startLogging() {
+    // Record metadata
+    Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+    Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+    Logger.recordMetadata("Git+SHA", BuildConstants.GIT_SHA);
+    Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+    Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+    switch (BuildConstants.DIRTY) {
+      case 0:
+        Logger.recordMetadata("GitDirty", "All changes committed");
+        break;
+      case 1:
+        Logger.recordMetadata("GitDirty", "Uncomitted changes");
+        break;
+      default:
+        Logger.recordMetadata("GitDirty", "Unknown");
+        break;
+    }
+
+    // Set up data receivers & replay source
+    switch (SimConstants.currentMode) {
+      case REAL:
+        // Running on a real robot, log to a USB stick ("/U/logs")
+        Logger.addDataReceiver(new WPILOGWriter());
+        Logger.addDataReceiver(new NT4Publisher());
+        break;
+
+      case SIM:
+        // Running a physics simulator, log to NT
+        Logger.addDataReceiver(new NT4Publisher());
+        break;
+
+      case REPLAY:
+        // Replaying a log, set up replay source
+        setUseTiming(false); // Run as fast as possible
+        String logPath = LogFileUtil.findReplayLog();
+        Logger.setReplaySource(new WPILOGReader(logPath));
+        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+        break;
+    }
+
+    // Start AdvantageKit logger
+    Logger.start();
   }
 }

src/main/java/frc4388/robot/RobotContainer.java

@@ -7,17 +7,17 @@
 
 package frc4388.robot;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.commands.PathPlannerAuto;
-
 // Drive Systems
 import edu.wpi.first.wpilibj.DriverStation;
+
+import java.io.File;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.utility.controller.XboxController;
+import frc4388.utility.controller.ButtonBox;
 import frc4388.utility.controller.DeadbandedXboxController;
-import frc4388.robot.Constants.OIConstants;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
@@ -25,67 +25,61 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-
 // Autos
 import frc4388.utility.controller.VirtualController;
-import frc4388.robot.commands.Swerve.neoJoystickPlayback;
-import frc4388.robot.commands.Swerve.neoJoystickRecorder;
-import frc4388.robot.subsystems.RobotLocalizer;
-// Subsystems
-// import frc4388.robot.subsystems.LED;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.robot.subsystems.TankDrive;
+import frc4388.robot.commands.wait.waitSupplier;
+import frc4388.robot.constants.Constants.OIConstants;
+import frc4388.robot.subsystems.differential.DiffDrive;
+
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
 // Utilites
 import frc4388.utility.DeferredBlock;
-import frc4388.utility.configurable.ConfigurableString;
+import frc4388.utility.compute.TimesNegativeOne;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
  * Command-based is a "declarative" paradigm, very little robot logic should
  * actually be handled in the {@link Robot} periodic methods (other than the
- * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * scheduler calls). Instead, the structure of the robot (2including subsystems,
  * commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
     /* RobotMap */
-    public final RobotMap m_robotMap = new RobotMap();
+    
+    public final RobotMap m_robotMap = RobotMap.configureReal();
     
     /* Subsystems */
-    // private final LED m_robotLED = new LED();
+    public final DiffDrive m_DiffDrive = new DiffDrive(m_robotMap.m_DiffDrive, m_robotMap.m_gyro);
+    // public final LED m_robotLED = new LED();
+    // public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
+    // public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED);
+    // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
+    // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
-    // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
-    //                                                               m_robotMap.rightFront,
-    //                                                               m_robotMap.leftBack,
-    //                                                               m_robotMap.rightBack,
-                                              
-    //                                                               m_robotMap.gyro);
-
-
-    // ! /*  Autos */
-    private final RobotLocalizer robotLocalizer = new RobotLocalizer(m_robotMap.gyro, m_robotMap.cam);
-    private final SendableChooser<Command> autoChooser;
-
-    
-    private final TankDrive tankDrive = new TankDrive(m_robotMap.FR, m_robotMap.FL, m_robotMap.BL, m_robotMap.BR, robotLocalizer);
 
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox   = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
     private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
-    private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
+    private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
 
-    /* Virtual Controllers */
-    private final VirtualController m_virtualDriver = new VirtualController(0);
-    private final VirtualController m_virtualOperator = new VirtualController(1);
+    // public List<Subsystem> subsystems = new ArrayList<>();
 
     // ! Teleop Commands
+    public void stop() {
+        // new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
+        // m_robotSwerveDrive.stopModules();
+        // Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
+    }
 
+    // ! /*  Autos */
+    private SendableChooser<String> autoChooser;
+    private Command autoCommand;
 
-    // private String lastAutoName = "defualt.auto";
-    // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
-    // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, 
-    // () -> autoplaybackName.get(), // lastAutoName
-    //        new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
-    //        true, false);
+    // private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove);
+    // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed);
+    // private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed);
+    private Command waitDebuger = new waitSupplier(() -> true);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -93,20 +87,32 @@ public class RobotContainer {
     public RobotContainer() {
         configureButtonBindings();        
         configureVirtualButtonBindings();
-        // new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip());
+        
+        DeferredBlock.addBlock(() -> { // Called on first robot enable
+            // m_robotSwerveDrive.resetGyro();
+        }, false);
+        DeferredBlock.addBlock(() -> { // Called on every robot enable
+            TimesNegativeOne.update();
+        }, true);
+
         DriverStation.silenceJoystickConnectionWarning(true);
         // CameraServer.startAutomaticCapture();
 
         /* Default Commands */
         // ! Swerve Drive Default Command (Regular Rotation)
         // drives the robot with a two-axis input from the driver controller
-        tankDrive.setDefaultCommand(new RunCommand(() -> {
-            tankDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
-                                            getDeadbandedDriverController().getRight(),
-                                true);
-        }, tankDrive)
+        m_DiffDrive.setDefaultCommand(new RunCommand(() -> {
+            m_DiffDrive.arcadeDrive(m_driverXbox.getLeft(), m_driverXbox.getRight());
+        }, m_DiffDrive)
         .withName("SwerveDrive DefaultCommand"));
-        // m_robotMap.tankDrive.setToSlow();
+
+        makeAutoChooser();
+        SmartDashboard.putData("Auto Chooser", autoChooser);
+        // this.subsystems.add(m_robotSwerveDrive);
+        // this.subsystems.add(m_robotMap.leftFront);
+        // this.subsystems.add(m_robotMap.rightFront);
+        // this.subsystems.add(m_robotMap.rightBack);
+        // this.subsystems.add(m_robotMap.leftBack);
 
         // ! Swerve Drive One Module Test
         // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
@@ -133,8 +139,7 @@ public class RobotContainer {
 
 
 
-        autoChooser = AutoBuilder.buildAutoChooser();
-        SmartDashboard.putData("Auto Chooser", autoChooser);
+
     }
 
     /**
@@ -145,25 +150,9 @@ public class RobotContainer {
      */
     private void configureButtonBindings() {
 
-        // ? /* Driver Buttons */
-
-        // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip()));
-            
-        // // ! /* Speed */
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
-        //     .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
-        
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
-
-        // new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
-
-        // new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
-          
-        // ?  /* Operator Buttons */
+        new JoystickButton(m_driverXbox, XboxController.A_BUTTON).onTrue(new InstantCommand(() -> {
+            m_DiffDrive.resetOdometry();
+        }));
 
         // ? /* Programer Buttons (Controller 3)*/
 
@@ -180,6 +169,7 @@ public class RobotContainer {
         //     new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
         //     true, false))
         //     .onFalse(new InstantCommand());
+
     }
     
     /**
@@ -214,8 +204,62 @@ public class RobotContainer {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        // return autoPlayback;
-        return autoChooser.getSelected();
+
+
+        //return autoPlayback;
+        //return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
+        //return autoChooser.getSelected();
+	// try{
+	// //     // Load the path you want to follow using its name in the GUI
+    //     return autoCommand;
+	// } catch (Exception e) {
+	//     DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
+	    return autoCommand;
+	// }
+    // return new PathPlannerAuto("Line-up-no-arm");
+	// zach told me to do the below comment
+	//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
+      //  return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
+    }
+
+    public boolean autoChooserUpdated = false;
+    public void makeAutoChooser() {
+        autoChooser = new SendableChooser<String>();
+        
+        File dir;
+
+        if(RobotBase.isReal()) {
+            dir = new File("/home/lvuser/deploy/pathplanner/autos/");
+        } else {
+            // dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
+            dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos");
+        }
+
+        String[] autos = dir.list();
+
+        if(autos == null) return;
+
+        for (String auto : autos) {
+            if (auto.endsWith(".auto"))
+                autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
+            // System.out.println(auto);
+        }
+
+        autoChooser.onChange((filename) -> {
+            autoChooserUpdated = true;
+            // if (filename.equals("Taxi")) {
+            //     autoCommand = new SequentialCommandGroup(
+            //         new MoveForTimeCommand(m_robotSwerveDrive, 
+            //             new Translation2d(0, -1), 
+            //             new Translation2d(), 1000, true
+            //     ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
+            // } else {
+                autoCommand = new PathPlannerAuto(filename);
+            // }
+            System.out.println("Robot Auto Changed " + filename);
+        });
+        // SmartDashboard.putData(autoChooser);
+
     }
 
     /**
@@ -236,11 +280,7 @@ public class RobotContainer {
         return this.m_operatorXbox;
     }
 
-    public VirtualController getVirtualDriverController() {
-        return m_virtualDriver;
-    }
-
-    public VirtualController getVirtualOperatorController() {
-        return m_virtualOperator;
+    public ButtonBox getButtonBox() {
+        return this.m_buttonBox;
     }
 }

src/main/java/frc4388/robot/RobotMap.java

@@ -7,76 +7,43 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.pathplanner.lib.commands.PathPlannerAuto;
-
-import edu.wpi.first.wpilibj2.command.Command;
-
-import org.photonvision.PhotonCamera;
-
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.Pigeon2;
 
-// import edu.wpi.first.wpilibj.motorcontrol.Spark;
-// import frc4388.robot.Constants.LEDConstants;
-import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.robot.Constants.VisionConstants;
-import frc4388.robot.subsystems.SwerveModule;
-import frc4388.robot.subsystems.TankDrive;
-import frc4388.utility.RobotGyro;
+import frc4388.robot.subsystems.differential.DiffConstants;
+import frc4388.robot.subsystems.differential.DiffIO;
+import frc4388.robot.subsystems.differential.DiffTalonSRX;
+import frc4388.robot.subsystems.differential.GyroIO;
+import frc4388.robot.subsystems.differential.GyroPigeon2;
 
 /**
  * Defines and holds all I/O objects on the Roborio. This is useful for unit
  * testing and modularization.
  */
 public class RobotMap {
-    private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON_ID);
-    public RobotGyro gyro = new RobotGyro(m_pigeon2);
-    public PhotonCamera cam = new PhotonCamera(VisionConstants.CAMERA_NAME);
+    // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
+    // public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
-    // public SwerveModule leftFront;
-    // public SwerveModule rightFront;
-    // public SwerveModule leftBack;
-    // public SwerveModule rightBack;
+    public DiffIO m_DiffDrive;
+    public GyroIO m_gyro;
 
-    public RobotMap() {
-        configureDriveMotorControllers();
+    public static RobotMap configureReal() {
+        RobotMap map = new RobotMap();
+
+        TalonSRX m_leftFront = new TalonSRX(DiffConstants.IDs.FRONT_LEFT_MOTOR.id);
+        TalonSRX m_rightFront = new TalonSRX(DiffConstants.IDs.FRONT_RIGHT_MOTOR.id);
+        TalonSRX m_leftRear = new TalonSRX(DiffConstants.IDs.REAR_LEFT_MOTOR.id);
+        TalonSRX m_rightRear = new TalonSRX(DiffConstants.IDs.REAR_RIGHT_MOTOR.id);
+
+        map.m_DiffDrive = new DiffTalonSRX(
+            m_leftFront, m_rightFront,
+            m_leftRear, m_rightRear
+        );
+
+        Pigeon2 pigeon2 = new Pigeon2(10);
+        map.m_gyro = new GyroPigeon2(pigeon2);
+
+        return map;
     }
    
-    public final TalonSRX FR = new TalonSRX(SwerveDriveConstants.IDs.FR_ID);
-    public final TalonSRX FL = new TalonSRX(SwerveDriveConstants.IDs.FL_ID);
-    public final TalonSRX BL = new TalonSRX(SwerveDriveConstants.IDs.BL_ID);
-    public final TalonSRX BR = new TalonSRX(SwerveDriveConstants.IDs.BR_ID);
-
-    public TankDrive tankDrive;
-
-    /* LED Subsystem */
-    // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
-
-    /* Swreve Drive Subsystem */
-    // public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
-    // public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
-    // public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
-
-
-    // public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
-    // public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
-    // public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
-    
-    // public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
-    // public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
-    // public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
-    
-    // public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
-    // public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
-    // public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
-
-    void configureDriveMotorControllers() {
-        // initialize SwerveModules
-        // this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
-        // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
-        // this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
-        // this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
-    }
 }

src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java

@@ -1,98 +0,0 @@
-package frc4388.robot.commands.Autos;
-
-import java.io.File;
-import java.util.ArrayList;
-import java.util.HashMap;
-
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc4388.robot.commands.Swerve.JoystickPlayback;
-import frc4388.robot.subsystems.SwerveDrive;
-
-public class PlaybackChooser {
-    private final ArrayList<SendableChooser<Command>> m_choosers    = new ArrayList<>();
-    private       SendableChooser<Command>            m_playback    = null;
-    private final ArrayList<ComplexWidget>            m_widgets     = new ArrayList<>();
-    private final HashMap<String, Command>            m_commandPool = new HashMap<>();
-    
-    private final File        m_dir    = new File("/home/lvuser/autos/");
-    private       int         m_cmdNum = 0;
-    private final SwerveDrive m_swerve;
-
-    // commands
-    private Command m_noAuto = new InstantCommand();
-    
-    public PlaybackChooser(SwerveDrive swerve, Object... pool) {
-        m_swerve = swerve;
-    }
-
-    public PlaybackChooser addOption(String name, Command option) {
-        m_commandPool.put(name, option);
-        return this;
-    }
-
-    public PlaybackChooser buildDisplay() {
-        for (int i = 0; i < 10; i++) {
-            appendCommand();
-        }
-        m_playback = m_choosers.get(0);
-        nextChooser();
-
-        // ! This does not work, why?
-        Shuffleboard.getTab("Auto Chooser")
-            .add("Add Sequence", new InstantCommand(() -> nextChooser()))
-            .withPosition(4, 0);
-        return this;
-    }
-
-    // This will be bound to a button for the time being
-    public void appendCommand() {
-        var chooser = new SendableChooser<Command>();
-        chooser.setDefaultOption("No Auto", m_noAuto);
-
-        m_choosers.add(chooser);
-        ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
-            .add("Command: " + m_choosers.size(), chooser)
-            .withSize(4, 1)
-            .withPosition(0, m_choosers.size() - 1)
-            .withWidget(BuiltInWidgets.kSplitButtonChooser);
-
-        m_widgets.add(widget);
-    }
-
-    public void nextChooser() {
-        SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
-
-        String[] dirs = m_dir.list();
-
-        if(dirs != null){ // Fix funny error
-            for (String auto : dirs) {
-                chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
-            }
-        }
-
-        
-        for (var cmd_name : m_commandPool.keySet()) {
-            chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
-        }
-    }
-
-    public Command getCommand() {
-        Command command = m_playback.getSelected();
-        command = command == null ? m_noAuto : command.asProxy();
-        
-        Command[] commands = new Command[m_cmdNum - 1];
-        for (int i = 0; i < m_cmdNum - 1; i++) {
-            Command command2 = m_choosers.get(i + 1).getSelected();
-            command2 = command2 == null ? m_noAuto : command2.asProxy();
-
-            commands[i] = command2.asProxy();
-        }
-
-        return command.andThen(commands);
-    }
-}

src/main/java/frc4388/robot/commands/Autos/Taxi.txt

@@ -1,225 +0,0 @@
-0.0,0.0,0.0,0.0,0
-0.0,0.0,0.0,0.0,0
-0.0,0.0,0.0,0.0,12
-0.0,0.0,0.0,0.0,26
-0.0,0.0,0.0,0.0,37
-0.0,0.0,0.0,0.0,50
-0.0,0.0,0.0,0.0,62
-0.0,0.0,0.0,0.0,73
-0.0,0.0,0.0,0.0,88
-0.0,0.0,0.0,0.0,103
-0.0,0.0,0.0,0.0,116
-0.0,0.0,0.0,0.0,160
-0.0,0.0,0.0,0.0,173
-0.0,0.0,0.0,0.0,185
-0.0,0.0,0.0,0.0,200
-0.0,0.0,0.0,0.0,211
-0.0,0.0,0.0,0.0,223
-0.0,0.0,0.0,0.0,235
-0.0,0.0,0.0,0.0,247
-0.0,0.0,0.0,0.0,263
-0.0,0.0,0.0,0.0,283
-0.0,0.0,0.0,0.0,303
-0.0,-0.109375,0.0,0.0,323
-0.0,-0.1484375,0.0,0.0,343
-0.0,-0.2109375,0.0,0.0,363
-0.0,-0.3671875,0.0,0.0,398
-0.0,-0.4140625,0.0,0.0,411
-0.0,-0.4765625,0.0,0.0,425
-0.0,-0.5078125,0.0,0.0,443
-0.0,-0.5078125,0.0,0.0,463
-0.0,-0.53125,0.0,0.0,483
-0.0,-0.5546875,0.0,0.0,504
-0.0,-0.5625,0.0,0.0,523
-0.0,-0.5625,0.0,0.0,544
-0.0,-0.5703125,0.0,0.0,563
-0.0,-0.5859375,0.0,0.0,584
-0.0,-0.5859375,0.0,0.0,603
-0.0,-0.5859375,0.0,0.0,640
-0.0,-0.59375,0.0,0.0,657
-0.0,-0.6015625,0.0,0.0,672
-0.0,-0.6015625,0.0,0.0,685
-0.0,-0.6015625,0.0,0.0,703
-0.0,-0.6015625,0.0,0.0,723
-0.0,-0.6015625,0.0,0.0,743
-0.0,-0.6015625,0.0,0.0,763
-0.0,-0.6015625,0.0,0.0,783
-0.0,-0.6015625,0.0,0.0,803
-0.0,-0.6015625,0.0,0.0,823
-0.0,-0.6015625,0.0,0.0,844
-0.0,-0.6015625,0.0,0.0,878
-0.0,-0.6015625,0.0,0.0,893
-0.0,-0.6015625,0.0,0.0,907
-0.0,-0.6015625,0.0,0.0,924
-0.0,-0.609375,0.0,0.0,943
-0.0,-0.609375,0.0,0.0,963
-0.0,-0.609375,0.0,0.0,983
-0.0,-0.609375,0.0,0.0,1004
-0.0,-0.609375,0.0,0.0,1023
-0.0,-0.609375,0.0,0.0,1043
-0.0,-0.609375,0.0,0.0,1064
-0.0,-0.609375,0.0,0.0,1083
-0.0,-0.609375,0.0,0.0,1156
-0.0,-0.609375,0.0,0.0,1172
-0.0,-0.609375,0.0,0.0,1185
-0.0,-0.609375,0.0,0.0,1200
-0.0,-0.609375,0.0,0.0,1215
-0.0,-0.609375,0.0,0.0,1225
-0.0,-0.609375,0.0,0.0,1236
-0.0,-0.609375,0.0,0.0,1249
-0.0,-0.609375,0.0,0.0,1263
-0.0,-0.609375,0.0,0.0,1283
-0.0,-0.609375,0.0,0.0,1303
-0.0,-0.609375,0.0,0.0,1323
-0.0,-0.609375,0.0,0.0,1363
-0.0,-0.6015625,0.0,0.0,1376
-0.0,-0.6015625,0.0,0.0,1394
-0.0,-0.6015625,0.0,0.0,1405
-0.0,-0.6015625,0.0,0.0,1423
-0.0,-0.6015625,0.0,0.0,1443
-0.0,-0.6015625,0.0,0.0,1463
-0.0,-0.6015625,0.0,0.0,1483
-0.0,-0.6015625,0.0,0.0,1503
-0.0,-0.6015625,0.0,0.0,1523
-0.0,-0.6015625,0.0,0.0,1543
-0.0,-0.6015625,0.0,0.0,1563
-0.0,-0.6015625,0.0,0.0,1597
-0.0,-0.6015625,0.0,0.0,1608
-0.0,-0.6015625,0.0,0.0,1624
-0.0,-0.6015625,0.0,0.0,1643
-0.0,-0.6015625,0.0,0.0,1664
-0.0,-0.5859375,0.0,0.0,1683
-0.0,-0.5859375,0.0,0.0,1703
-0.0,-0.5625,0.0,0.0,1723
-0.0,-0.5625,0.0,0.0,1743
-0.0,-0.5625,0.0,0.0,1763
-0.0,-0.5625,0.0,0.0,1783
-0.0,-0.5625,0.0,0.0,1803
-0.0,-0.5625,0.0,0.0,1843
-0.0,-0.5625,0.0,0.0,1855
-0.0,-0.5625,0.0,0.0,1868
-0.0,-0.5625,0.0,0.0,1883
-0.0,-0.5625,0.0,0.0,1903
-0.0,-0.5625,0.0,0.0,1923
-0.0,-0.5625,0.0,0.0,1943
-0.0,-0.5625,0.0,0.0,1963
-0.0,-0.5625,0.0,0.0,1983
-0.0,-0.5625,0.0,0.0,2003
-0.0,-0.5625,0.0,0.0,2024
-0.0,-0.5625,0.0,0.0,2043
-0.0,-0.5625,0.0,0.0,2081
-0.0,-0.5625,0.0,0.0,2093
-0.0,-0.5625,0.0,0.0,2105
-0.0,-0.5625,0.0,0.0,2123
-0.0,-0.5625,0.0,0.0,2143
-0.0,-0.5625,0.0,0.0,2163
-0.0,-0.5625,0.0,0.0,2183
-0.0,-0.5625,0.0,0.0,2203
-0.0,-0.5625,0.0,0.0,2223
-0.0,-0.5625,0.0,0.0,2243
-0.0,-0.5625,0.0,0.0,2263
-0.0,-0.5625,0.0,0.0,2283
-0.0,-0.5625,0.0,0.0,2366
-0.0,-0.5625,0.0,0.0,2377
-0.0,-0.5625,0.0,0.0,2394
-0.0,-0.5703125,0.0,0.0,2405
-0.0,-0.5703125,0.0,0.0,2418
-0.0,-0.5703125,0.0,0.0,2431
-0.0,-0.5703125,0.0,0.0,2444
-0.0,-0.5703125,0.0,0.0,2458
-0.0,-0.5703125,0.0,0.0,2470
-0.0,-0.5703125,0.0,0.0,2485
-0.0,-0.5703125,0.0,0.0,2503
-0.0,-0.5703125,0.0,0.0,2523
-0.0,-0.5703125,0.0,0.0,2563
-0.0,-0.5703125,0.0,0.0,2577
-0.0,-0.5703125,0.0,0.0,2591
-0.0,-0.5703125,0.0,0.0,2608
-0.0,-0.5703125,0.0,0.0,2624
-0.0,-0.5703125,0.0,0.0,2643
-0.0,-0.5703125,0.0,0.0,2677
-0.0,-0.5703125,0.0,0.0,2698
-0.0,-0.5703125,0.0,0.0,2711
-0.0,-0.5703125,0.0,0.0,2725
-0.0,-0.5703125,0.0,0.0,2743
-0.0,-0.5703125,0.0,0.0,2764
-0.0,-0.5703125,0.0,0.0,2810
-0.0,-0.5703125,0.0,0.0,2820
-0.0,-0.5703125,0.0,0.0,2833
-0.0,-0.5703125,0.0,0.0,2845
-0.0,-0.5703125,0.0,0.0,2863
-0.0,-0.5703125,0.0,0.0,2883
-0.0,-0.5703125,0.0,0.0,2904
-0.0,-0.5703125,0.0,0.0,2924
-0.0,-0.5703125,0.0,0.0,2943
-0.0,-0.5703125,0.0,0.0,2963
-0.0,-0.5703125,0.0,0.0,2983
-0.0,-0.5703125,0.0,0.0,3003
-0.0,-0.5703125,0.0,0.0,3033
-0.0,-0.5703125,0.0,0.0,3050
-0.0,-0.5703125,0.0,0.0,3065
-0.0,-0.5703125,0.0,0.0,3083
-0.0,-0.5703125,0.0,0.0,3103
-0.0,-0.5703125,0.0,0.0,3123
-0.0,-0.5703125,0.0,0.0,3144
-0.0,-0.5703125,0.0,0.0,3164
-0.0,-0.5703125,0.0,0.0,3184
-0.0,-0.5703125,0.0,0.0,3203
-0.0,-0.5703125,0.0,0.0,3223
-0.0,-0.5703125,0.0,0.0,3243
-0.0,-0.5703125,0.0,0.0,3272
-0.0,-0.5703125,0.0,0.0,3289
-0.0,-0.5703125,0.0,0.0,3303
-0.0,-0.5703125,0.0,0.0,3323
-0.0,-0.5703125,0.0,0.0,3343
-0.0,-0.5703125,0.0,0.0,3363
-0.0,-0.5703125,0.0,0.0,3383
-0.0,-0.5703125,0.0,0.0,3403
-0.0,-0.5703125,0.0,0.0,3423
-0.0,-0.5703125,0.0,0.0,3443
-0.0,-0.5703125,0.0,0.0,3463
-0.0,-0.5703125,0.0,0.0,3483
-0.0,-0.5703125,0.0,0.0,3566
-0.0,-0.5703125,0.0,0.0,3578
-0.0,-0.5703125,0.0,0.0,3596
-0.0,-0.5703125,0.0,0.0,3610
-0.0,-0.5703125,0.0,0.0,3623
-0.0,-0.5703125,0.0,0.0,3640
-0.0,-0.5703125,0.0,0.0,3651
-0.0,-0.5703125,0.0,0.0,3663
-0.0,-0.5703125,0.0,0.0,3678
-0.0,-0.5703125,0.0,0.0,3691
-0.0,-0.5703125,0.0,0.0,3706
-0.0,-0.5703125,0.0,0.0,3723
-0.0,-0.5703125,0.0,0.0,3766
-0.0,-0.5703125,0.0,0.0,3778
-0.0,-0.5703125,0.0,0.0,3792
-0.0,-0.5703125,0.0,0.0,3807
-0.0,-0.5703125,0.0,0.0,3823
-0.0,-0.5703125,0.0,0.0,3843
-0.0,-0.53125,0.0,0.0,3863
-0.0,-0.53125,0.0,0.0,3884
-0.0,-0.421875,0.0,0.0,3904
-0.0,0.0,0.0,0.0,3924
-0.0,0.0,0.0,0.0,3944
-0.0,0.0,0.0,0.0,3963
-0.0,0.0,0.0,0.0,3999
-0.0,0.0,0.0,0.0,4010
-0.0,0.0,0.0,0.0,4025
-0.0,0.0,0.0,0.0,4043
-0.0,0.0,0.0,0.0,4063
-0.0,0.0,0.0,0.0,4083
-0.0,0.0,0.0,0.0,4103
-0.0,0.0,0.0,0.0,4123
-0.0,0.0,0.0,0.0,4143
-0.0,0.0,0.0,0.0,4163
-0.0,0.0,0.0,0.0,4183
-0.0,0.0,0.0,0.0,4203
-0.0,0.0,0.0,0.0,4236
-0.0,0.0,0.0,0.0,4247
-0.0,0.0,0.0,0.0,4264
-0.0,0.0,0.0,0.0,4284
-0.0,0.0,0.0,0.0,4304
-0.0,0.0,0.0,0.0,4324
-0.0,0.0,0.0,0.0,4343
-0.0,0.0,0.0,0.0,4363

src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java

@@ -1,3 +1,4 @@
+package frc4388.robot.commands.autos;
 // package frc4388.robot.commands.Autos;
 
 // import java.io.File;

src/main/java/frc4388/robot/commands/MoveForTimeCommand.java

@@ -0,0 +1,49 @@
+// package frc4388.robot.commands;
+
+// import java.time.Instant;
+
+// import edu.wpi.first.math.geometry.Translation2d;
+// import edu.wpi.first.wpilibj2.command.Command;
+// import frc4388.robot.subsystems.SwerveDrive;
+
+// // Command to repeat a joystick movement for a specific time.
+// public class MoveForTimeCommand extends Command {
+//     private final SwerveDrive swerveDrive;
+//     private final Translation2d leftStick;
+//     private final Translation2d rightStick;
+//     private final long duration;
+//     private final boolean robotRelative;
+
+//     private Instant startTime;
+
+//     public MoveForTimeCommand(
+//         SwerveDrive swerveDrive, 
+//         Translation2d leftStick, 
+//         Translation2d rightStick, 
+//         long millis,
+//         boolean robotRelative) {
+        
+//         addRequirements(swerveDrive);
+
+//         this.swerveDrive = swerveDrive;
+//         this.leftStick = leftStick;
+//         this.rightStick = rightStick;
+//         this.duration = millis;
+//         this.robotRelative = robotRelative;
+//     }
+
+//     @Override
+//     public void initialize() {
+//         startTime = Instant.now();
+//     }
+
+//     @Override
+//     public void execute() {
+//         swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
+//     }
+
+//     @Override
+//     public boolean isFinished() {
+//         return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
+//     }
+// }

src/main/java/frc4388/robot/commands/MoveUntilSuply.java

@@ -0,0 +1,45 @@
+// package frc4388.robot.commands;
+
+// import java.util.function.Supplier;
+
+// import edu.wpi.first.math.geometry.Translation2d;
+// import edu.wpi.first.wpilibj2.command.Command;
+// import frc4388.robot.subsystems.SwerveDrive;
+
+// // Command to repeat a joystick movement for a specific time.
+// public class MoveUntilSuply extends Command {
+//     private final SwerveDrive swerveDrive;
+//     private final Translation2d leftStick;
+//     private final Translation2d rightStick;
+//     private final Supplier<Boolean> truth;
+//     private final boolean robotRelative;
+
+//     public MoveUntilSuply(
+//         SwerveDrive swerveDrive, 
+//         Translation2d leftStick, 
+//         Translation2d rightStick, 
+//         Supplier<Boolean> truth,
+//         boolean robotRelative) {
+//             addRequirements(swerveDrive);
+
+//         this.swerveDrive = swerveDrive;
+//         this.leftStick = leftStick;
+//         this.rightStick = rightStick;
+//         this.truth = truth;
+//         this.robotRelative = robotRelative;
+//     }
+
+//     @Override
+//     public void initialize() {
+//     }
+
+//     @Override
+//     public void execute() {
+//         swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
+//     }
+
+//     @Override
+//     public boolean isFinished() {
+//         return truth.get();
+//     }
+// }

src/main/java/frc4388/robot/commands/PID.java

@@ -5,7 +5,7 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.utility.Gains;
+import frc4388.utility.structs.Gains;
 
 public abstract class PID extends Command {
 	protected Gains  gains;

src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java

@@ -1,145 +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 frc4388.robot.commands.Swerve;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.util.ArrayList;
-import java.util.Scanner;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.UtilityStructs.TimedOutput;
-
-public class JoystickPlayback extends Command {
-  private final SwerveDrive            swerve;
-  private       String                 filename;
-  private       int                    mult       = 1;
-  private       Scanner                input;
-  private final ArrayList<TimedOutput> outputs      = new ArrayList<>();
-  private       int                    counter      = 0;
-  private       long                   startTime    = 0;
-  private       long                   playbackTime = 0;
-  private       int                    lastIndex;
-  private       boolean                m_finished   = false; // ! find a better way
-
-  /** Creates a new JoystickPlayback. */
-  public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
-    this.swerve = swerve;
-    this.filename = filename;
-    this.mult = mult;
-
-    addRequirements(this.swerve);
-  }
-
-  /** Creates a new JoystickPlayback. */
-  public JoystickPlayback(SwerveDrive swerve, String filename) {
-    this(swerve, filename, 1);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    outputs.clear();
-    m_finished = false;
-
-    startTime = System.currentTimeMillis();
-    playbackTime = 0;
-    lastIndex    = 0;
-    try {
-      input = new Scanner(new File("/home/lvuser/autos/" + filename));
-
-      String line = "";
-      while (input.hasNextLine()) {
-        line = input.nextLine();
-
-        if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
-          continue;
-        }
-      
-        String[] values = line.split(",");
-        System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
-
-        var out = new TimedOutput();
-        out.leftX  = Double.parseDouble(values[0]) * mult;
-        out.leftY  = Double.parseDouble(values[1]);
-        out.rightX = Double.parseDouble(values[2]);
-        out.rightY = Double.parseDouble(values[3]);
-
-        out.timedOffset = Long.parseLong(values[4]);
-
-        outputs.add(out);
-      }
-
-      input.close();
-    } catch (FileNotFoundException e) {
-      e.printStackTrace();
-    }
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    if (counter == 0) {
-      startTime = System.currentTimeMillis();
-      playbackTime = 0;
-    } else {
-      playbackTime = System.currentTimeMillis() - startTime;
-    }
-
-    // skip to reasonable time frame
-    // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
-    {
-      int i = lastIndex == 0 ? 1 : lastIndex;
-      while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
-        i++;
-      }
-
-      if (i >= outputs.size()) {
-        m_finished = true; // ! kind of a hack
-        return;
-      }
-      lastIndex = i;
-    }
-
-    TimedOutput lastOut = outputs.get(lastIndex - 1);
-    TimedOutput out     = outputs.get(lastIndex);
-
-    double deltaTime     = out.timedOffset - lastOut.timedOffset;
-    double playbackDelta = playbackTime    - lastOut.timedOffset;
-
-    double lerpLX = lastOut.leftX  + (out.leftX  - lastOut.leftX)  * (playbackDelta / deltaTime);
-    double lerpLY = lastOut.leftY  + (out.leftY  - lastOut.leftY)  * (playbackDelta / deltaTime);
-    double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
-    double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
-
-    // this.swerve.driveWithInput(new Translation2d(out.leftX,   out.leftY),
-    //                              new Translation2d(out.rightX, out.rightY),
-    //                              true);
-    
-    // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
-    //                             new Translation2d(lerpRX, lerpRY),
-    //                             true);
-
-    this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
-                                new Translation2d(lerpRX, lerpRY),
-                                true);
-                             
-    counter++;
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    input.close();
-    swerve.stopModules();
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return m_finished;
-  }
-}

src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java

@@ -1,97 +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 frc4388.robot.commands.Swerve;
-
-import java.io.File;
-import java.io.IOException;
-import java.io.PrintWriter;
-import java.util.ArrayList;
-import java.util.function.Supplier;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.UtilityStructs.TimedOutput;
-
-public class JoystickRecorder extends Command {
-  public  final SwerveDrive            swerve;
-
-  public  final Supplier<Double>       leftX;
-  public  final Supplier<Double>       leftY;
-  public  final Supplier<Double>       rightX;
-  public  final Supplier<Double>       rightY;
-  private       String                 filename;
-  public  final ArrayList<TimedOutput> outputs   = new ArrayList<>();
-  private       long                   startTime = -1;
-
-
-  /** Creates a new JoystickRecorder. */
-  public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX,  Supplier<Double> leftY,
-                                              Supplier<Double> rightX, Supplier<Double> rightY,
-                                              String filename)
-  {
-    this.swerve = swerve;
-    this.leftX  = leftX;
-    this.leftY  = leftY;
-    this.rightX = rightX;
-    this.rightY = rightY;
-    this.filename = filename;
-
-    addRequirements(this.swerve);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    outputs.clear();
-
-    this.startTime = System.currentTimeMillis();
-
-    outputs.add(new TimedOutput());
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    var inputs = new TimedOutput();
-    inputs.leftX       = leftX.get();
-    inputs.leftY       = leftY.get();
-    inputs.rightX      = rightX.get();
-    inputs.rightY      = rightY.get();
-    inputs.timedOffset = System.currentTimeMillis() - startTime;
-
-    outputs.add(inputs);
-
-    swerve.playbackDriveWithInput(new Translation2d(inputs.leftX,  inputs.leftY),
-                          new Translation2d(inputs.rightX, inputs.rightY),
-                          true);
-    
-    //System.out.println("RECORDING");
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    File output = new File("/home/lvuser/autos/" + filename);
-
-    try (PrintWriter writer = new PrintWriter(output)) {
-      for (var input : outputs) {
-        writer.println( input.leftX  + "," + input.leftY  + "," +
-                        input.rightX + "," + input.rightY + "," +
-                        input.timedOffset);
-      }
-
-      writer.close();
-    } catch (IOException e) {
-        e.printStackTrace();
-    }
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java

@@ -2,11 +2,11 @@
 // 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 frc4388.robot.commands.Swerve;
+package frc4388.robot.commands.swerve;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import frc4388.robot.commands.PID;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 public class RotateToAngle extends PID {
 

src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java

@@ -1,4 +1,4 @@
-package frc4388.robot.commands.Swerve;
+package frc4388.robot.commands.swerve;
 
 import java.io.FileInputStream;
 import java.util.ArrayList;
@@ -6,11 +6,11 @@ import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.DataUtils;
-import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
-import frc4388.utility.UtilityStructs.AutoRecordingFrame;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+import frc4388.utility.compute.DataUtils;
 import frc4388.utility.controller.VirtualController;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
 
 
 /**
@@ -24,8 +24,8 @@ public class neoJoystickPlayback extends Command {
     private final Supplier<String>              filenameGetter;
     private       String                        filename;
     private       int                           frame_index    = 0;
-    private       long                          startTime      = 0;
-    private       long                          playbackTime   = 0;
+    // private       long                          startTime      = 0;
+    // private       long                          playbackTime   = 0;
     private       boolean                       m_finished     = false; // ! There is no better way.
     private       boolean                       m_shouldfree   = false; // should free memory on ending
     
@@ -150,8 +150,8 @@ public class neoJoystickPlayback extends Command {
 
     @Override
     public void initialize() {
-        startTime = System.currentTimeMillis();
-        playbackTime = 0;
+        // startTime = System.currentTimeMillis();
+        // playbackTime = 0;
         frame_index = 0;
 
         m_finished = !loadAuto();

src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java

@@ -1,4 +1,4 @@
-package frc4388.robot.commands.Swerve;
+package frc4388.robot.commands.swerve;
 
 import java.io.FileOutputStream;
 import java.util.ArrayList;
@@ -7,11 +7,11 @@ import java.util.function.Supplier;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.DataUtils;
-import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
-import frc4388.utility.UtilityStructs.AutoRecordingFrame;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+import frc4388.utility.compute.DataUtils;
 import frc4388.utility.controller.DeadbandedXboxController;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
 
 /**
  * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s 

src/main/java/frc4388/robot/commands/WhileTrueCommand.java

@@ -0,0 +1,104 @@
+package frc4388.robot.commands;
+
+// 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.
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.function.BooleanSupplier;
+
+/**
+ * A command composition that runs one of two commands, depending on the value of the given
+ * condition when this command is initialized.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class WhileTrueCommand extends Command {
+  private final Command m_whileTrue;
+  private final BooleanSupplier m_condition;
+
+  /**
+   * Creates a new WhileTrueCommand.
+   *
+   * @param whileTrue the command to run while the condition is true
+   * @param condition the condition to determine which command to run
+   */
+  @SuppressWarnings("this-escape")
+  public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
+    m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
+    m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
+
+    //CommandScheduler.getInstance().registerComposedCommands(whileTrue);
+
+    // addRequirements(whileTrue.getRequirements());
+  }
+
+  @Override
+  public void initialize() {
+    if(m_condition.getAsBoolean())
+      m_whileTrue.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_whileTrue.execute();
+
+    System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " +  m_condition.getAsBoolean());
+
+    if(!m_whileTrue.isFinished())
+      return;
+
+    if(m_condition.getAsBoolean()){
+      m_whileTrue.end(false);
+      m_whileTrue.initialize();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_whileTrue.end(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_whileTrue.runsWhenDisabled();
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+      return InterruptionBehavior.kCancelSelf;
+    } else {
+      return InterruptionBehavior.kCancelIncoming;
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
+    builder.addStringProperty(
+        "selected",
+        () -> {
+          if (m_whileTrue == null) {
+            return "null";
+          } else {
+            return m_whileTrue.getName();
+          }
+        },
+        null);
+  }
+}

src/main/java/frc4388/robot/commands/alignment/DriveToReef.java

@@ -0,0 +1,188 @@
+package frc4388.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+import frc4388.robot.subsystems.vision.Vision;
+import frc4388.utility.compute.ReefPositionHelper;
+import frc4388.utility.compute.TimesNegativeOne;
+import frc4388.utility.compute.ReefPositionHelper.Side;
+import frc4388.utility.structs.Gains;
+
+public class DriveToReef extends Command {
+    
+
+    // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
+    // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
+
+    private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
+    private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
+    // private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
+    private Pose2d targetpos;
+
+    SwerveDrive swerveDrive;
+    Vision vision;
+    double distance;
+    Side side;
+    boolean waitVelocityZero;
+
+    /**
+     * Command to drive robot to position.
+     * @param SwerveDrive m_robotSwerveDrive
+     */
+
+    public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
+        this.swerveDrive = swerveDrive;
+        this.vision = vision;
+        this.distance = distance;
+        this.side = side;
+        this.waitVelocityZero = waitVelocityZero && false;
+        addRequirements(swerveDrive);
+    }
+
+
+    public static double tagRelativeXError = -1;
+    private static void setTagRelativeXError(double val){
+        tagRelativeXError = val;
+    }
+
+    @Override
+    public void initialize() {
+        xPID.initialize();
+        yPID.initialize();
+        this.targetpos = ReefPositionHelper.getNearestPosition(
+            this.vision.getPose2d(), 
+            side, 
+            Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), 
+            distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
+        );
+    }
+    
+    double xerr;
+    double yerr;
+    double roterr;
+
+    double xoutput;
+    double youtput;
+    double rotoutput;
+
+    @Override
+    public void execute() {
+        xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
+        yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
+        // xerr = targetpos.getX() - vision.getPose2d().getX();
+        // yerr = targetpos.getX() - vision.getPose2d().getY();
+
+        // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
+
+        roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
+
+        if(roterr > 180){
+            roterr -= 360;
+        }else if(roterr < -180){
+            roterr += 360;
+        }
+
+        // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
+        // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
+        // SmartDashboard.putNumber("Rotational PID error", roterr);
+
+        SmartDashboard.putNumber("PID X Error", xerr);
+        SmartDashboard.putNumber("PID Y Error", yerr);
+        SmartDashboard.putNumber("PID Rot Error", roterr);
+
+        xoutput = xPID.update(xerr);
+        youtput = yPID.update(yerr);
+        // rotoutput = rotPID.update(roterr);
+
+        xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
+        youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
+        // rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
+        
+
+
+        Translation2d leftStick = new Translation2d(
+            Math.max(Math.min(-youtput, 1), -1),
+            Math.max(Math.min(-xoutput, 1), -1)
+            // 0,0
+        );
+
+        // Translation2d rightStick = new Translation2d(
+        //     Math.max(Math.min(rotoutput, 1), -1), 
+        //    0
+        // );
+
+        setTagRelativeXError(
+            new Translation2d(xerr, yerr).getAngle()
+            .rotateBy(targetpos.getRotation())
+            .getCos());
+
+        swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
+        // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
+    }
+
+    @Override
+    public final boolean isFinished() {
+        boolean finished = (
+            (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
+            (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
+        );
+        // System.out.println(finished);
+
+        if(finished)
+            swerveDrive.softStop();
+
+        return finished;
+                // this statement is a boolean in and of itself
+    }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    private class PID {
+        protected Gains  gains;
+        private   double output    = 0;
+
+
+        /** Creates a new PelvicInflammatoryDisease. */
+        public PID(Gains gains, double tolerance) {
+            this.gains     = gains;
+        }
+
+        // Called when the command is initially scheduled.
+        public final void initialize() {
+            output = 0;
+        }
+
+        private double prevError, cumError = 0;
+        
+        // Called every time the scheduler runs while the command is scheduled.
+        public double update(double error) {
+            cumError += error * .02; // 20 ms
+            double delta = error - prevError;
+
+            output = error * gains.kP;
+            output += cumError * gains.kI;
+            output += delta * gains.kD;
+            output += gains.kF;
+
+            return output;
+        }
+    }
+}

src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java

@@ -0,0 +1,48 @@
+package frc4388.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+// Command to repeat a joystick movement for a specific time.
+public class DriveUntilLiDAR extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final LiDAR m_lidar;
+    private final double mindistance;
+
+    public DriveUntilLiDAR(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        LiDAR lidar,
+        double mindistance) {
+            addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.m_lidar = lidar;
+        this.mindistance = mindistance;
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveFine(leftStick, rightStick, 0.3);
+    }
+
+    @Override
+    public boolean isFinished() {
+        if (Math.abs(m_lidar.getDistance()) < mindistance) {
+            swerveDrive.softStop();
+            return true;
+        }
+        return false;
+    }
+}

src/main/java/frc4388/robot/commands/alignment/LidarAlign.java

@@ -0,0 +1,107 @@
+// 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 frc4388.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class LidarAlign extends Command {
+  private SwerveDrive swerveDrive;
+  private LiDAR lidar;  
+
+  private int currentFinderTick;
+  // private int tickFoundPipe;
+  private boolean foundReef;
+  private boolean headedRight;
+  private double speed;
+  private int bounces;
+  private double additionalDistance = 0;
+  // private final boolean constructedHeadedRight;
+
+  /** Creates a new LidarAlign. */
+  public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.swerveDrive = swerveDrive;
+    this.lidar = lidar;
+
+    addRequirements(swerveDrive, lidar);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.currentFinderTick = 0;
+    this.speed = 0.4; // TODO: find good speed for this
+    this.foundReef = false;
+    this.headedRight = (DriveToReef.tagRelativeXError < 0);
+    this.additionalDistance = 0;
+    this.bounces = 0;
+  }
+
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (lidar.withinDistance()) {
+      swerveDrive.softStop(); 
+      foundReef = true;
+      return;
+    }
+
+    if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
+      headedRight = !headedRight;
+      currentFinderTick *= -1;
+      bounces++;
+      additionalDistance += 5;
+      if (bounces == 5) return;
+    }
+    double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
+    double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
+    SmartDashboard.putNumber("Rel Angle", relAngle);
+    SmartDashboard.putNumber("heading", currentHeading);
+    if (!headedRight) {
+      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
+    } else {
+      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
+    }
+
+    currentFinderTick++;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (lidar.getDistance() < 4) {
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && lidar.withinDistance()) { // spot on
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && !lidar.withinDistance()) { // over shot
+      speed = speed / 2;
+      headedRight = !headedRight;
+      currentFinderTick = 0;
+      foundReef = false;
+      return false;
+    } else if (bounces >= 3) {
+      swerveDrive.stopModules();
+      return true;
+    } else {
+      return false;
+    }
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java

@@ -0,0 +1,36 @@
+// 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 frc4388.robot.commands.wait;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Elevator;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitElevatorRefrence extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitElevatorRefrence(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.elevatorAtReference();
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java

@@ -0,0 +1,36 @@
+// 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 frc4388.robot.commands.wait;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Elevator;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitEndefectorRefrence extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitEndefectorRefrence(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.endeffectorAtReference();
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java

@@ -0,0 +1,36 @@
+// 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 frc4388.robot.commands.wait;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Elevator;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitFeedCoral extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitFeedCoral(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.hasCoral();
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitSupplier.java

@@ -0,0 +1,36 @@
+// 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 frc4388.robot.commands.wait;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitSupplier extends Command {
+  /** Creates a new waitSupplier. */
+  private final Supplier<Boolean> truth;
+  public waitSupplier(Supplier<Boolean> truth) {
+    this.truth = truth;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return truth.get();
+  }
+}

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -0,0 +1,19 @@
+package frc4388.robot.constants;
+
+/**
+ * Automatically generated file containing build version information.
+ */
+public final class BuildConstants {
+  public static final String MAVEN_GROUP = "";
+  public static final String MAVEN_NAME = "Vision-Minibot";
+  public static final String VERSION = "unspecified";
+  public static final int GIT_REVISION = 5;
+  public static final String GIT_SHA = "a9e45fcf9ba356f63af7c567d51115d255bb269b";
+  public static final String GIT_DATE = "2024-12-16 00:39:59 MST";
+  public static final String GIT_BRANCH = "master";
+  public static final String BUILD_DATE = "2025-07-16 14:12:56 MDT";
+  public static final long BUILD_UNIX_TIME = 1752696776235L;
+  public static final int DIRTY = 1;
+
+  private BuildConstants(){}
+}

src/main/java/frc4388/robot/constants/Constants.java

@@ -0,0 +1,234 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.constants;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotBase;
+import frc4388.utility.compute.Trim;
+import frc4388.utility.status.CanDevice;
+import frc4388.utility.structs.Gains;
+import frc4388.utility.structs.LEDPatterns;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+    public static final String CANBUS_NAME = "rio";
+
+    public static final class LiDARConstants {
+        public static final int REEF_LIDAR_DIO_CHANNEL = 7;
+        public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
+
+        public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
+
+        public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
+        public static final int LIDAR_MICROS_TO_CM = 10;
+        public static final int SECONDS_TO_MICROS = 1000000;
+    }
+
+    public static final class AutoConstants {
+        // public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
+        public static final Gains XY_GAINS = new Gains(8,0,0.0);
+        // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
+        // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
+        // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
+       // public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
+
+        // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
+        // public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
+
+        public static final Trim X_OFFSET_TRIM =        new Trim("X Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
+        // public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5);
+        public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
+        public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
+        public static final Trim ARM_OFFSET_TRIM =      new Trim("ARM Offset Trim",      -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
+                
+        public static final double XY_TOLERANCE = 0.07; // Meters
+        public static final double ROT_TOLERANCE = 5; // Degrees
+
+        public static final double MIN_XY_PID_OUTPUT = 0.0;
+        public static final double MIN_ROT_PID_OUTPUT = 0.0;
+
+        public static final double VELOCITY_THRESHHOLD = 0.01;
+                
+        // X is tangent to reef side
+        // Y is normal to reef side
+        public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
+        public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
+        public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
+
+        public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
+        public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
+        // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
+        
+        public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
+        public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
+        
+        public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
+        public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2);
+
+        public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
+        
+        // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
+        // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
+        
+        // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
+        
+        // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
+
+        // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
+
+        public static final int L4_DRIVE_TIME = 250; //Milliseconds
+        // public static final int L3_DRIVE_TIME = 500;
+        public static final int L2_DRIVE_TIME = 250; //Milliseconds
+        public static final int ALGAE_DRIVE_TIME = 500;
+        public static final double STOP_VELOCITY = 0.0;
+    }
+
+    public static final class VisionConstants { 
+        public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
+        public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
+
+        public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
+        public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
+        
+        public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
+
+        // Photonvision thing
+        // The standard deviations of our vision estimated poses, which affect correction rate
+        // X, Y, Theta
+        // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
+        public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
+        public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
+    }
+
+    public static final class FieldConstants {
+        public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
+
+        // Test april tag field layout
+        // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
+        //     Arrays.asList(new AprilTag[] {
+        //         new AprilTag(1, new Pose3d(
+        //             new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
+        //         )),
+        //     }), 100, 100);
+
+    }
+
+    public static final class LEDConstants {
+        public static final int LED_SPARK_ID = 9;
+
+        public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
+
+        public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
+        public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
+        public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
+        public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
+
+        public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
+        public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
+    }
+
+    public static final class OIConstants {
+        public static final int XBOX_DRIVER_ID = 0;
+        public static final int XBOX_OPERATOR_ID = 1;
+        public static final int BUTTONBOX_ID = 2;
+        public static final int XBOX_PROGRAMMER_ID = 3;
+        public static final double LEFT_AXIS_DEADBAND = 0.1;
+
+    }
+
+    public static final class ElevatorConstants {
+        public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
+        public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
+
+        public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75;
+        public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20;
+
+        // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
+        
+        public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
+        public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
+        public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND
+
+        
+        public static final double GEAR_RATIO_ELEVATOR = -9.0;
+        //Max for elevator = 50%
+        
+        public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
+        public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
+        public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
+        public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
+        public static final double SCORING_THREE_ELEVATOR = -9.25;
+        public static final double DEALGAE_L2_ELEVATOR = -4;
+        public static final double DEALGAE_L3_ELEVATOR = -26.5;
+        public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
+        
+        public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
+        public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
+        //Max for endefector = 60%
+        public static final double L2_SCORE_ELEVATOR = -5;
+        public static final double L2_LEAVE_ELEVATOR = -11;
+
+        public static final double L2_SCORE_ENDEFFECTOR = -19;
+
+        public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
+        public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
+        public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
+        public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
+        public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
+        public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
+        public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
+
+        public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
+            .withKP(1)
+            .withKI(0)
+            .withKD(0);
+        
+        public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs()
+            .withKP(1)
+            .withKI(0)
+            .withKD(0);
+    }
+
+    // Logging constants
+    public static class SimConstants {
+        public static final Mode simMode = Mode.SIM;
+        public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
+
+        public static enum Mode {
+            /** Running on a real robot. */
+            REAL,
+
+            /** Running a physics simulator. */
+            SIM,
+
+            /** Replaying from a log file. */
+            REPLAY
+        }
+    }
+}

src/main/java/frc4388/robot/subsystems/Apriltags.java

@@ -1,36 +0,0 @@
-package frc4388.robot.subsystems;
-
-//import edu.wpi.first.apriltag.AprilTag;
-//import edu.wpi.first.math.geometry.Pose3d;
-//import edu.wpi.first.math.geometry.Rotation3d;
-//import edu.wpi.first.networktables.NetworkTable;
-//import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
-public class Apriltags {
-    public static class Tag {
-        public boolean visible = true;
-        public double x,  y,  z  = 0;
-        public double ry, rp, rr = 0;
-    }
-
-    public Tag getTagPosRot() {
-        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
-
-        final Tag tag = new Tag();
-        tag.visible = isAprilTag();
-        tag.x  = tagTable.getEntry("TagPosX").getDouble(0);
-        tag.y  = tagTable.getEntry("TagPosY").getDouble(0);
-        tag.z  = tagTable.getEntry("TagPosZ").getDouble(0);
-        tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
-        tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
-        tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
-
-        return tag;
-    }
-
-    public boolean isAprilTag() {
-        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
-        return tagTable.getEntry("IsTag").getBoolean(false);
-    }
-}

src/main/java/frc4388/robot/subsystems/DiffDrive.java

@@ -1,89 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.subsystems;
-
-import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.hardware.core.CorePigeon2;
-
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import frc4388.robot.Constants.DriveConstants;
-import frc4388.utility.RobotGyro;
-import frc4388.utility.RobotTime;
-
-/**
- * Add your docs here.
- */
-public class DiffDrive extends SubsystemBase {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-  private RobotTime m_robotTime = RobotTime.getInstance();
-
-  private TalonFX m_leftFrontMotor;
-  private TalonFX m_rightFrontMotor;
-  private TalonFX m_leftBackMotor;
-  private TalonFX m_rightBackMotor;
-  private DifferentialDrive m_driveTrain;
-  private RobotGyro m_gyro;
-
-  /**
-   * Add your docs here.
-   */
-  public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
-      TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
-
-    m_leftFrontMotor = leftFrontMotor;
-    m_rightFrontMotor = rightFrontMotor;
-    m_leftBackMotor = leftBackMotor;
-    m_rightBackMotor = rightBackMotor;
-    m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
-    m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
-    m_driveTrain = driveTrain;
-    m_gyro = gyro;
-  }
-
-  @Override
-  public void periodic() {
-    m_gyro.updatePigeonDeltas();
-
-    if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
-      updateSmartDashboard();
-    }
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(move, steer);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void tankDriveWithInput(double leftMove, double rightMove) {
-    m_leftFrontMotor.set(leftMove);
-    m_rightFrontMotor.set(rightMove);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  private void updateSmartDashboard() {
-
-    // Examples of the functionality of RobotGyro
-    SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
-    SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
-    SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
-    //SmartDashboard.putData(m_gyro);
-  }
-}

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

@@ -0,0 +1,397 @@
+// 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 frc4388.robot.subsystems;
+
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.constants.Constants.ElevatorConstants;
+import frc4388.robot.constants.Constants.LEDConstants;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status.ReportLevel;
+
+public class Elevator extends SubsystemBase implements Queryable {
+  /** Creates a new Elevator. */
+  private TalonFX elevatorMotor;
+  private TalonFX endeffectorMotor;
+  private LED led;
+
+  // @AutoLog
+  // private class ElevatorState {
+    @SuppressWarnings("unused")
+    public long wait = 0;
+    public long maxWait = 1000;
+
+    public double elevatorRefrence = 0;
+    public double endeffectorRefrence = 0;
+
+    public boolean elevatorManualStop = true;
+    public boolean endefectorManualStop = true;
+
+    public boolean disableAutoIntake = false;
+
+    public boolean seededZeroEndefector = false;
+    public boolean seededZeroElevator = false;
+
+    public DigitalInput basinBeamBreak;
+    public DigitalInput endeffectorLimitSwitch;
+    public DigitalInput intakeIR;
+  // }
+
+  // private ElevatorState state = new ElevatorState();
+
+  public enum CoordinationState {
+    Waiting, // for coral into the though
+    WatingBeamTripped, //once the beam break trips
+    Ready, // Has coral in endefector
+    Hovering, // Has coral in endefector
+    L2Score,
+    L2ScoreLeave,
+    PrimedThree, // Arm and elevator Waiting to score in the level 3 position
+    ScoringThree, // Arm and elevator in the level three position
+    PrimedFour, // Arm and elevator Waiting to score in the level 4 position
+    ScoringFour, // Arm and elevator in the level four position
+    BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef.
+    BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef.
+    BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef.
+    BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef.
+  }
+
+  private CoordinationState currentState;
+
+  // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
+  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
+    elevatorMotor = elevatorTalonFX;
+    endeffectorMotor = endeffectorTalonFX;
+    this.led = led;
+
+    this.basinBeamBreak = basinLimitSwitch;
+    this.endeffectorLimitSwitch = endeffectorLimitSwitch;
+    this.intakeIR = intakeDigitalInput;
+
+    elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
+    endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
+    
+    elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
+    endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
+    currentState = CoordinationState.Ready;
+
+    FaultReporter.register(this);
+  }
+
+  //PID methods
+
+  private void PIDPosition(TalonFX motor, double position) {
+    if (motor == elevatorMotor) elevatorRefrence = position;
+    else endeffectorRefrence = position;
+
+    var request = new PositionDutyCycle(position);
+    motor.setControl(request);
+  }
+
+  public void elevatorStop() {
+    elevatorMotor.set(0);
+  }
+
+  public void endeffectorStop() {
+    endeffectorMotor.set(0);
+  }
+
+
+  public void transitionState(CoordinationState state) {
+    // elevatorMotor.enable();
+    
+
+    currentState = state;
+    switch (currentState) {
+      case Waiting: {
+        wait = System.currentTimeMillis() + maxWait;
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
+        led.setMode(LEDConstants.WAITING_PATTERN);
+        break;
+      }
+
+      case WatingBeamTripped: {
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.DOWN_PATTERN);
+        break;
+      }
+
+      case Ready: {
+        PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.DOWN_PATTERN);
+        break;
+      }
+
+      case Hovering: {
+        PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.READY_PATTERN);
+        break;
+      }
+
+      case L2Score: {
+        PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case L2ScoreLeave: {
+        PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+      
+      case PrimedFour: {
+        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case ScoringFour: {
+        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case PrimedThree: {
+        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+      
+      case ScoringThree: {
+        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case BallRemoverL2Primed: {
+        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case BallRemoverL2Go: {
+        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case BallRemoverL3Primed: {
+        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
+        break;
+      }
+
+      case BallRemoverL3Go: {
+        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      default: {
+        assert false;
+      }
+    }
+
+  }
+
+  public void togggleAutoIntaking() {
+    disableAutoIntake = !disableAutoIntake;
+  }
+
+  public boolean elevatorAtReference() {
+    // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
+    double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
+    double diffrence = elevatorRefrence - elevatorPosition;
+
+    boolean headedUp = diffrence < 0;
+    boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
+    boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
+  }
+
+  public boolean endeffectorAtReference() {
+    // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
+    double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
+    double diffrence = endeffectorRefrence - endeffectorPosition;
+
+    boolean headedUp = diffrence < 0;
+    boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
+    boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
+  }
+  // public void driveElevatorStick(Translation2d stick) {
+  //   if (stick.getNorm() > 0.05) {
+  //     elevatorMotor.set(stick.getY());
+  //   }
+  // }
+
+  public boolean getEndeffectorLimit() {
+    return endeffectorLimitSwitch.get();
+  }
+
+  private void periodicWaiting() {
+    if (!basinBeamBreak.get()) 
+      transitionState(CoordinationState.Ready);
+    // if(!endeffectorLimitSwitch.get())
+    //   transitionState(CoordinationState.Hovering);
+  }
+
+  // private void periodicWaitingTripped() {
+  //   if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) 
+  //     transitionState(CoordinationState.Ready);
+  // }
+  
+  private void periodicReady() {
+    if (elevatorAtReference() && !endeffectorLimitSwitch.get())
+      transitionState(CoordinationState.Hovering);
+    if(elevatorAtReference() && endeffectorLimitSwitch.get())
+      transitionState(CoordinationState.Hovering);
+  }
+
+  @SuppressWarnings("unused")
+  private void periodicScoring() {
+    if (!endeffectorLimitSwitch.get()) 
+      transitionState(CoordinationState.Waiting);
+  }
+
+  public void manualElevatorVel(double velocity) {
+    if (Math.abs(velocity) > 0.1) {
+      elevatorMotor.set(velocity);
+      elevatorManualStop = false;
+      return;
+    }
+    if (!elevatorManualStop) {
+      elevatorManualStop = true;
+      elevatorMotor.set(0);
+    }
+  }
+
+  public void manualEndeffectorVel(double velocity) {
+    if (Math.abs(velocity) > 0.1) {
+      endeffectorMotor.set(velocity);
+      endefectorManualStop = false;
+      return;
+    }
+    if (!endefectorManualStop) {
+      endefectorManualStop = true;
+      endeffectorMotor.set(0);
+    }
+  }
+
+  @Override
+  public void periodic() {
+
+    // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble();
+    // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble();
+    // double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
+    // double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
+
+
+    // if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
+    //   PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
+    // }
+
+    // This method will be called once per scheduler run
+    // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
+    // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
+    SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
+    SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
+    SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
+    SmartDashboard.putString("State", currentState.toString());
+
+    if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) {
+      endeffectorMotor.setPosition(0);
+      seededZeroEndefector = !seededZeroEndefector;
+    }
+
+    if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) {
+      elevatorMotor.setPosition(0);
+      seededZeroElevator = !seededZeroElevator;
+    }
+    
+    if (disableAutoIntake) return;
+
+    if (currentState == CoordinationState.Waiting) {
+      periodicWaiting();
+    } else if (currentState == CoordinationState.WatingBeamTripped) {
+      // periodicWaitingTripped();
+    } else if (currentState == CoordinationState.Ready) {
+      periodicReady();
+    }
+
+    if(!intakeIR.get()){
+      led.setMode(LEDConstants.DOWN_PATTERN);
+    }
+
+    
+    // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
+    //   periodicScoring();
+    // }
+  }
+
+  public boolean isL4Primed() {
+    return currentState == CoordinationState.PrimedFour;
+  }
+
+  public boolean isL3Primed() {
+    return currentState == CoordinationState.PrimedThree;
+  }
+
+  public boolean hasCoral() {
+    return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
+  }
+
+  public boolean readyToMove() {
+    return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get();
+    // return hasCoral();
+  }
+
+  public void armShuffle(){
+    if(!basinBeamBreak.get()){
+      //shuffle the coral with the arm until coral hits beam break
+    }
+  }
+
+  @Override
+  public String getName() {
+    return "Elevator";
+  }
+
+  // @Override
+  // public void queryStatus() {}
+
+  @Override
+  public Status diagnosticStatus() {
+    Status status = new Status();
+
+    status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name());
+
+    return status;
+  }
+}

src/main/java/frc4388/robot/subsystems/LED.java

@@ -7,84 +7,69 @@
 
 package frc4388.robot.subsystems;
 
-import edu.wpi.first.wpilibj.AddressableLED;
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.AutoLogOutput;
 
-import frc4388.robot.Constants.LEDConstants;
-import frc4388.utility.LEDPatterns;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.LEDConstants;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status.ReportLevel;
+import frc4388.utility.structs.LEDPatterns;
 
 /**
  * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
  * Driver
  */
-public class LED extends SubsystemBase {
-
-  static AddressableLED m_led;
-  static AddressableLEDBuffer m_ledBuffer;
-  static LED m_self;
-  /**
-   * Add your docs here.
-   */
-
-  public LED(){
-    if(m_self != null)
-      return;
-    m_led = new AddressableLED(9);
-    m_ledBuffer = new AddressableLEDBuffer(10);
-    m_led.setLength(m_ledBuffer.getLength());
-    m_led.setData(m_ledBuffer);
-    m_led.start();
-    System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
+public class LED extends SubsystemBase implements Queryable {
+  public LED() {
+    FaultReporter.register(this);
   }
-  public static LED getInstance() {
-    if(m_self == null)
-      m_self = new LED();
-    return m_self;
+
+  private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+  private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN;
+
+  public void setMode(LEDPatterns pattern){
+    this.mode = pattern;
   }
+
   @Override
-  public void periodic(){
-    //gamermode();
-    //SmartDashboard.putNumber("LED", m_currentPattern.getValue());
-    return;
-  }
-  static int firstcolor = 0;
-  static void gamermode() {
-      for(int i = 0; i < m_ledBuffer.getLength(); i++) {
-          final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
-          setLEDHSV(i, hue, 255, 128);
-      }
-      firstcolor +=3;
-      firstcolor %= 180;
-  }
-  /**
-   * Add your docs here.
-   */
-  public static void updateLED (){
-    gamermode();
-   // m_LEDController.set(m_currentPattern.getValue());
+  public void periodic() {
+    update();
   }
 
-  /**
-   * Add your docs here.
-   */
-  public static void setLEDRGB(int lednum, int r, int g, int b){
-    m_ledBuffer.setRGB(lednum, r, g, b);
-    //m_currentPattern = pattern;
-   // m_LEDController.set(m_currentPattern.getValue());
+  public void update() {
+    if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return;
+
+    if(DriverStation.isDisabled()){
+      LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue());
+    }else
+      LEDController.set(mode.getValue());
   }
-  public static void setLEDHSV(int lednum, int hue, int sat, int val){
-    m_ledBuffer.setRGB(lednum, hue, sat, val);
-    //m_currentPattern = pattern;
-   // m_LEDController.set(m_currentPattern.getValue());
+
+  @AutoLogOutput
+  public String state() {
+    return mode.getClass().toString();
   }
-  /**
-   * Add your docs here.
-   * @return
-   */
-  public AddressableLEDBuffer getBuffer() {
-    return m_ledBuffer;
+
+  @Override
+  public String getName() {
+    return "LEDs";
   }
+
+  @Override
+  public Status diagnosticStatus() {
+    Status status = new Status();
+
+    if(!LEDController.isAlive())
+      status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
+    
+    status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
+
+    return status;
+  }
+  
+
 }

src/main/java/frc4388/robot/subsystems/RobotLocalizer.java

@@ -1,106 +0,0 @@
-package frc4388.robot.subsystems;
-
-import org.photonvision.PhotonCamera;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-import com.pathplanner.lib.path.PathPlannerPath;
-import com.pathplanner.lib.util.DriveFeedforwards;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.utility.RobotGyro;
-
-public class RobotLocalizer extends SubsystemBase {
-    private RobotGyro gyro;
-    private Pose2d lastPose2d = new Pose2d();
-    private PhotonCamera camera;
-
-
-    public RobotLocalizer(RobotGyro gyro, PhotonCamera cam) {
-        this.gyro = gyro;
-        this.camera = cam;
-    }
-
-    @Override
-    public void periodic() {
-        // time
-        Translation3d accel = gyro.getAcceleration3d();
-
-        SmartDashboard.putNumber("Accel X", accel.getX());
-        SmartDashboard.putNumber("Accel Y", accel.getY());
-        SmartDashboard.putNumber("Accel Z", accel.getZ());
-
-        Rotation3d rot = gyro.getRotation3d();
-
-        SmartDashboard.putNumber("Rot X", rot.getX());
-        SmartDashboard.putNumber("Rot Y", rot.getY());
-        SmartDashboard.putNumber("Rot Z", rot.getZ());
-
-        // boolean tagExists = SmartDashboard.getBoolean("photonvision/Camera_Module_v1/hasTarget", false);
-        
-        Translation2d pos;
-
-        var result = camera.getAllUnreadResults();
-        // if (result.hasTargets()) {
-        //     PhotonTrackedTarget target = result.getBestTarget();
-        //     Transform3d pos3d = target.getBestCameraToTarget();
-        //     pos = new Translation2d(pos3d.getX(), pos3d.getY());
-        // } else {
-            pos = lastPose2d.getTranslation();
-        // }
-
-        // results.
-        // if (!results.isEmpty()) {
-
-        //     double totalX = 0;
-        //     double totalY = 0;
-
-        //     // Camera processed a new frame since last
-        //     // Get the last one in the list.
-        //     var result = results.get(results.size() - 1);
-        //     // PhotonTrackedTarget targets = result.getMultiTagResult();
-
-        //     if (result.hasTargets()) {
-        //         PhotonTrackedTarget target = result.getBestTarget();
-        //         Transform3d pos3d = target.getBestCameraToTarget();
-        //         pos = new Translation2d(pos3d.getX(), pos3d.getY());
-        //     } else {
-        //         pos = lastPose2d.getTranslation();
-        //     }
-        // }else {
-        //     pos = lastPose2d.getTranslation();
-        // }
-
-
-        lastPose2d = new Pose2d(
-            pos,
-            gyro.getRotation2d() 
-        );
-    }
-
-    // Pathplanner function
-    public Pose2d getPose(){
-        return lastPose2d;
-    }
-
-    // PathPlanner func
-    public void resetPose(Pose2d pose){
-        lastPose2d = pose;
-    }
-
-    // PathPlanner
-    public ChassisSpeeds getChassisSpeeds() {
-        return new ChassisSpeeds(
-            0, 
-            0, 
-            Units.rotationsToRadians(gyro.getAngularVelocity())
-        );
-    }
-}

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -1,333 +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 frc4388.robot.subsystems;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.robot.Constants.SwerveDriveConstants.Conversions;
-import frc4388.utility.RobotGyro;
-import frc4388.utility.RobotUnits;
-
-public class  SwerveDrive extends SubsystemBase {
-  
-  private SwerveModule leftFront;
-  private SwerveModule rightFront;
-  private SwerveModule leftBack;
-  private SwerveModule rightBack;
-
-  private SwerveModule[] modules;
-
-  private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
-  private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
-  private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
-  private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
-  
-  private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
-  
-  private RobotGyro gyro;
-  
-  private int gear_index;
-  private boolean stopped = false;
-
-  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
-  public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
-  public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
-  
-  public double rotTarget = 0.0;
-  public Rotation2d orientRotTarget = new Rotation2d();
-  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
-
-  /** Creates a new SwerveDrive. */
-  public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
-    this.leftFront = leftFront;
-    this.rightFront = rightFront;
-    this.leftBack = leftBack;
-    this.rightBack = rightBack;
-    
-    this.gyro = gyro;
-    reset_index();
-    this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
-  }
-
-  public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
-    // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
-    // rightStick.getAngle()
-    double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
-    // System.out.println(ang);
-    // module.go(ang);
-    // Rotation2d rot = Rotation2d.fromRadians(ang);
-    Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
-    SwerveModuleState state = new SwerveModuleState(speed, rot);
-    module.setDesiredState(state);
-  }
-
-  public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-
-    double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
-    SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
-
-    if (fieldRelative) {
-
-      double rot = 0;
-      
-      // ! drift correction
-      if (rightStick.getNorm() > 0.05) {
-        rotTarget = gyro.getAngle();
-        rot_correction = 0;
-        // rot = rightStick.getX();
-      //  SmartDashboard.putBoolean("drift correction", false);
-        stopped = false;
-      } else if(leftStick.getNorm() > 0.05) {
-        if (!stopped) {
-          stopModules();
-          stopped = true;
-        }
-
-     //   SmartDashboard.putBoolean("drift correction", true);
-        
-        // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
-
-      }
-
-      // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
-      Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-      // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
-
-      // Convert field-relative speeds to robot-relative speeds.
-      // chassisSpeeds = chassisSpeeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
-    } else {      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
-    }
-    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
-  }
-
-  public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-    if (fieldRelative) {
-
-      double rot = 0;
-      
-      // ! drift correction
-      if (rightStick.getNorm() > 0.05) {
-        rotTarget = gyro.getAngle();
-        rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
-      //  SmartDashboard.putBoolean("drift correction", false);
-        stopped = false;
-      } else if(leftStick.getNorm() > 0.05) {
-        if (!stopped) {
-          stopModules();
-          stopped = true;
-        }
-
-     //   SmartDashboard.putBoolean("drift correction", true);
-        // double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
-        
-
-      }
-
-      // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
-      Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
-      // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
-
-      // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
-    } else {      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
-    }
-    // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
-  }
-
-  public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-
-    // Translation2d rightStick = new Translation2d(-rightX, rightY);
-    double rightX = rightStick.getX();
-    double rightY = rightStick.getY();
-
-    double rot_correction = 0;
-
-    // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
-
-    if(fieldRelative) {
-      double rot = 0;
-      if(rightStick.getNorm() > 0.5) {
-        orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
-        
-        Rotation2d tmp =  orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
-        double min = tmp.getDegrees();
-        min = Math.max(Math.abs(min), 2);
-        if(tmp.getDegrees() < 0)
-          min*=-1;
-        tmp = new Rotation2d(min * Math.PI / 180);
-        rot = tmp.getRadians(); // x   x - y/x
-      }
-    
-    Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1);
-    } else {      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
-    }
-    // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));  
-    }
-
-  /**
-   * Set each module of the swerve drive to the corresponding desired state.
-   * @param desiredStates Array of module states to set.
-   */
-  public void setModuleStates(SwerveModuleState[] desiredStates) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
-    for (int i = 0; i < desiredStates.length; i++) {
-      SwerveModule module = modules[i];
-      SwerveModuleState state = desiredStates[i];
-      module.setDesiredState(state);
-    }
-  }
-
-  public boolean rotateToTarget(double angle) {
-    double currentAngle = getGyroAngle();
-    double error = angle - currentAngle;
-
-    driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
-
-    if (Math.abs(angle - getGyroAngle()) < 5.0) {
-      return true;
-    }
-
-    return false;
-  }
-
-  public double getGyroAngle() {
-    return -gyro.getAngle();
-  }
-
-  public void add180() {
-    gyro.reset(gyro.getAngle()+180);
-    rotTarget = gyro.getAngle();
-
-  }
-
-  public void resetGyro() {
-    gyro.reset();
-    rotTarget = gyro.getAngle();
-  }
-
-  public void resetGyroFlip() {
-    gyro.resetFlip();
-    rotTarget = gyro.getAngle();
-  }
-
-  public void resetGyroRightBlue() {
-    gyro.resetRightSideBlue();
-    rotTarget = gyro.getAngle();
-  }
-
-  public void resetGyroRightAmp() {
-    gyro.resetAmpSide();
-    rotTarget = gyro.getAngle();
-  }
-  
-  public void stopModules() {
-    for (SwerveModule module : this.modules) {
-      module.stop();
-    }
-  }
-
-  public SwerveDriveKinematics getKinematics() {
-    return this.kinematics;
-  }
-
-  public boolean getSpeedState() {
-    
-    return false;
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run\
-    SmartDashboard.putNumber("Gyro", getGyroAngle());
-    SmartDashboard.putNumber("RotTartget", rotTarget);
-  }
-
-  private void reset_index() {
-    gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?)
-  }
-
-  public void shiftDown() {
-    if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
-    int i = gear_index - 1;
-    if (i == -1) i = 0;
-    setPercentOutput(SwerveDriveConstants.GEARS[i]);
-    gear_index = i;
-  }
-
-  public void shiftUp() {
-    if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
-    int i = gear_index + 1;
-    if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
-    setPercentOutput(SwerveDriveConstants.GEARS[i]);
-    gear_index = i;
-  }
-
-  public void setPercentOutput(double speed) {
-    speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed;
-    gear_index = -1;
-  }
-
-  public void setToSlow() {
-    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
-    System.out.println("SLOW");
-    System.out.println("SLOW");
-    System.out.println("SLOW");
-    System.out.println("SLOW");
-    System.out.println("SLOW");
-  }
-
-  public void setToFast() {
-    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
-    System.out.println("FAST");
-    System.out.println("FAST");
-    System.out.println("FAST");
-    System.out.println("FAST");
-    System.out.println("FAST");
-  }
-
-  public void setToTurbo() {
-    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED;
-    System.out.println("TURBO");
-    System.out.println("TURBO");
-    System.out.println("TURBO");
-    System.out.println("TURBO");
-    System.out.println("TURBO");
-  }
-
-  public void toggleGear(double angle) {
-    if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
-      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
-      SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
-    } else {
-      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
-      SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
-    }
-  }
-
-  public void shiftUpRot() {
-    rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
-  }
-
-  public void shiftDownRot() {
-    rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
-  }
-
-  
-
-}

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -1,242 +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 frc4388.robot.subsystems;
-
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.Utils;
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
-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.OpenLoopRampsConfigs;
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.ctre.phoenix6.signals.SensorDirectionValue;
-import com.ctre.phoenix6.hardware.CANcoder;
-
-import edu.wpi.first.math.geometry.Translation2d;
-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.math.util.Units;
-// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.utility.Gains;
-
-public class SwerveModule extends SubsystemBase {
-    private TalonFX driveMotor;
-    private TalonFX angleMotor;
-    private CANcoder encoder;
-    // private final StatusSignal<Double> cc_pos;
-    // private final StatusSignal<Double> cc_vel;
-    // private int selfid;
-    // private ConfigurableDouble offsetGetter;
-    private static int swerveId = 0;
-    public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
-  
-    
-    /** Creates a new SwerveModule. */
-    public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
-        this.driveMotor = driveMotor;
-        this.angleMotor = angleMotor;
-        this.encoder = encoder;
-
-        var motorCfg = new TalonFXConfiguration()
-            .withOpenLoopRamps(
-                new OpenLoopRampsConfigs()
-                    .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
-            ).withClosedLoopRamps(
-                new ClosedLoopRampsConfigs()
-                    .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
-            ).withMotorOutput(
-                new MotorOutputConfigs()
-                    .withNeutralMode(NeutralModeValue.Brake)
-                    .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
-            ).withCurrentLimits(
-                new CurrentLimitsConfigs()
-                    .withStatorCurrentLimit(100)
-                    .withStatorCurrentLimitEnable(true)
-                    .withSupplyCurrentLimit(100)
-                    .withSupplyCurrentLimitEnable(true)
-            );
-
-        driveMotor.getConfigurator().apply(motorCfg);
-        
-        TalonFXConfiguration angleConfig = new TalonFXConfiguration()
-            .withOpenLoopRamps(
-                new OpenLoopRampsConfigs()
-                    .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
-            ).withClosedLoopRamps(
-                new ClosedLoopRampsConfigs()
-                    .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
-            ).withMotorOutput(
-                new MotorOutputConfigs()
-                    .withNeutralMode(NeutralModeValue.Brake)
-                    .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
-            );
-
-        angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-
-        angleConfig.Slot0.kP = swerveGains.kP;
-        angleConfig.Slot0.kI = swerveGains.kI;   
-        angleConfig.Slot0.kD = swerveGains.kD;
-
-        angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID();
-        angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
-        angleMotor.getConfigurator().apply(angleConfig);
-
-        CANcoderConfiguration canconfig = new CANcoderConfiguration();
-        canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
-        canconfig.MagnetSensor.MagnetOffset = offset;
-        encoder.getConfigurator().apply(canconfig);
-
-        rotateToAngle(0);
-    }
-
-    // public void go(double ang){
-    //     // double curang = this.encoder.getAbsolutePosition().getValue();
-    //     System.out.println(getAngle().getDegrees());
-    //     rotateToAngle(ang);
-    // }
-    
-    @Override
-    public void periodic() {
-        //encoder.configMagnetOffset(offsetGetter.get());
-        //SmartDashboard.putString("Error Code: " + selfid, getstuff());
-        // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
-        // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
-        // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
-        // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());
-    }
-    /**
-     * Get the drive motor of the SwerveModule
-     * @return the drive motor of the SwerveModule
-     */
-    public TalonFX getDriveMotor() {
-        return this.driveMotor;
-    }
-
-    /**
-     * Get the angle motor of the SwerveModule
-     * @return the angle motor of the SwerveModule
-     */
-    public TalonFX getAngleMotor() {
-        return this.angleMotor;
-    }
-
-    /**
-     * Get the CANcoder of the SwerveModule
-     * @return the CANcoder of the SwerveModule
-     */
-    public CANcoder getEncoder() {
-        return this.encoder;
-    }
-
-    /**
-     * Get the angle of a SwerveModule as a Rotation2d
-     * @return the angle of a SwerveModule as a Rotation2d
-     */
-    public Rotation2d getAngle() {
-        // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
-        // return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
-        return Rotation2d.fromRotations(encoder.getPosition().getValue().baseUnitMagnitude());
-    }
-    
-    public double getAngularVel() {
-        // return this.angleMotor.getSelectedSensorVelocity();
-        return angleMotor.getVelocity().getValueAsDouble();
-    }
-
-    public double getDrivePos() {
-        // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
-        return driveMotor.getPosition().getValueAsDouble();
-    }
-
-    public double getDriveVel() {
-        // return this.driveMotor.getSelectedSensorVelocity(0);
-        return driveMotor.getVelocity().getValueAsDouble();
-    }
-
-    public void stop() {
-        driveMotor.set(0);
-        angleMotor.set(0);
-    }
-
-    public void rotateToAngle(double angle) {
-        final PositionVoltage m_request = new PositionVoltage(angle);
-        angleMotor.setControl(m_request);
-    }
-
-    /**
-     * Get state of swerve module
-     * @return speed in m/s and angle in degrees
-     */
-    public SwerveModuleState getState() {
-        return new SwerveModuleState(
-            Units.inchesToMeters(driveMotor.getVelocity().getValue().baseUnitMagnitude() * 
-            SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV * 
-            SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV), 
-            getAngle()
-        );
-    }
-
-    // private SwerveModuleState optimizeState(SwerveModuleState desiredState) {
-    //     Rotation2d curRot = this.getAngle();
-        
-    // }
-
-    /**
-     * Returns the current position of the SwerveModule
-     * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
-    //  */
-    // public SwerveModulePosition getPosition() {
-    //     return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
-    // }
-
-    /**
-     * Set the speed and rotation of the SwerveModule from a SwerveModuleState object
-     * @param desiredState a SwerveModuleState representing the desired new state of the module
-    //  */
-    public void setDesiredState(SwerveModuleState desiredState) {
-        Rotation2d currentRotation = this.getAngle();
-
-        SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
-
-        // calculate the difference between our current rotational position and our new rotational position
-        Rotation2d rotationDelta = state.angle.minus(currentRotation);
-
-        double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND;
-
-        rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations());
-
-        driveMotor.set(Math.max(Math.min(speed, 1.), -1.));
-    }
-
-    public void reset() {
-        // encoder.setPosition(0);
-    }
-
-    // public double getCurrent() {
-    //     return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
-    // }
-
-    // public double getVoltage() {
-    //     return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
-    // }
-
-    // public String getstuff() {
-    //     encoder.getPosition();
-    //     return "" + encoder.getLastError().value;
-    // }
-}

src/main/java/frc4388/robot/subsystems/TankDrive.java

@@ -1,115 +0,0 @@
-package frc4388.robot.subsystems;
-
-import java.util.function.BooleanSupplier;
-import java.util.function.Function;
-
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.controllers.PPLTVController;
-import com.pathplanner.lib.path.PathPlannerPath;
-import com.pathplanner.lib.util.DriveFeedforwards;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
-
-public class TankDrive extends SubsystemBase{
-    private TalonSRX FR;
-    private TalonSRX FL;
-    private TalonSRX BL;
-    private TalonSRX BR;
-    private RobotLocalizer robotLocalizer;
-
-    public TankDrive(TalonSRX FR,TalonSRX FL,TalonSRX BL, TalonSRX BR, RobotLocalizer robotLocalizer){
-        this.FR = FR;
-        this.FL = FL;
-        this.BL = BL;
-        this.BR = BR;
-        this.robotLocalizer = robotLocalizer;
-
-        
-        // Configure AutoBuilder last
-        AutoBuilder.configure(
-            robotLocalizer::getPose,
-            robotLocalizer::resetPose,
-            robotLocalizer::getChassisSpeeds,
-            this::setTargetChassisSpeeds,
-            AutoConstants.PP_PATH_FOLLOWING_CONTROLLER,
-            AutoConstants.PP_ROBOT_CONFIG,
-            () -> {
-                // Boolean supplier that controls when the path will be mirrored for the red alliance
-                // This will flip the path being followed to the red side of the field.
-                // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-    
-                var alliance = DriverStation.getAlliance();
-                if (alliance.isPresent()) {
-                    return alliance.get() == DriverStation.Alliance.Red;
-                }
-                return false;
-            },
-            this
-        );
-    }
-
-    private static final ControlMode mode = ControlMode.PercentOutput;
-
-    private static final double maxMoveSpeed = 0.3;
-    private static final double maxturnSpeed = 0.3;
-
-
-    private static final double[] turn = { //Right by default, motors are wired weirdly
-        maxturnSpeed, //FR
-        maxturnSpeed, //FL
-        maxturnSpeed, //BL
-        maxturnSpeed, //BR
-    };
-
-    private static final double[] move = { //forward by default, motors are wired weirdly
-        -maxMoveSpeed,  //FR
-        maxMoveSpeed,  //FL
-        maxMoveSpeed,  //BL
-        -maxMoveSpeed,  //BR
-    };
-
-    public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-
-        double percent_move = /* (1. - Math.abs(rightStick.getX())) */ leftStick.getY();
-
-        double FR_rot = move[0] * percent_move + turn[0] * rightStick.getX();
-        double FL_rot = move[1] * percent_move + turn[1] * rightStick.getX();
-        double BL_rot = move[2] * percent_move + turn[2] * rightStick.getX();
-        double BR_rot = move[3] * percent_move + turn[3] * rightStick.getX();
-
-        FR.set(mode, FR_rot);
-        FL.set(mode, FL_rot);
-        BL.set(mode, BL_rot);
-        BR.set(mode, BR_rot);
-
-        SmartDashboard.putNumber("FR", FR_rot);
-        SmartDashboard.putNumber("FL", FL_rot);
-        SmartDashboard.putNumber("BL", BL_rot);
-        SmartDashboard.putNumber("BR", BR_rot);
-
-    }
-
-    private double clamp(double x, double min, double max){
-        return Math.max(Math.min(x, max), min);
-    }
-
-    private void setTargetChassisSpeeds(ChassisSpeeds target, DriveFeedforwards idk) {
-        double move = clamp(target.vxMetersPerSecond/AutoConstants.MAX_DRIVE_VELOCITY, -1, 1);
-        double rot = clamp(Units.radiansToRotations(target.omegaRadiansPerSecond)/AutoConstants.MAX_ROT_SPEED, -1, 1);
-
-        driveWithInput(
-            new Translation2d(0, move), 
-            new Translation2d(rot, 0),
-            false);
-
-    }
-}

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

@@ -1,38 +0,0 @@
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.apriltag.AprilTag;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
-public class Vision {
-    private final NetworkTableEntry m_isTags;
-    private final NetworkTableEntry m_xPoses;
-    private final NetworkTableEntry m_yPoses;
-    private final NetworkTableEntry m_zPoses;
-
-    public Vision() {
-        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
-
-        m_isTags = tagTable.getEntry("IsTag");
-        m_xPoses = tagTable.getEntry("TagPosX");
-        m_yPoses = tagTable.getEntry("TagPosY");
-        m_zPoses = tagTable.getEntry("TagPosZ");
-    }
-
-    public AprilTag[] getAprilTags() {
-        if (!m_isTags.getBoolean(false)) return new AprilTag[0];
-
-        double xarr[] = m_xPoses.getDoubleArray(new double[] {});
-        double yarr[] = m_yPoses.getDoubleArray(new double[] {});
-        double zarr[] = m_zPoses.getDoubleArray(new double[] {});
-
-        AprilTag tags[] = new AprilTag[xarr.length];
-        for (int i = 0; i < tags.length; i++) {
-            tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
-        }
-
-        return tags;
-    }
-}

src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java

@@ -0,0 +1,29 @@
+package frc4388.robot.subsystems.differential;
+
+import static edu.wpi.first.units.Units.Inches;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Distance;
+import frc4388.utility.status.CanDevice;
+import frc4388.utility.structs.Gains;
+
+public class DiffConstants {
+    
+    public static final Distance WHEEL_RADIUS_TO_ARC = Inches.of(2.5).times(Math.PI * 2); //meters
+    public static final Distance TRACK_DISPLACEMENT = Inches.of(6.5); //meters
+    public static final Distance TRACK_WIDTH = TRACK_DISPLACEMENT.times(2);
+
+    public static final Gains ROT_GAINS = new Gains(20, 0, 0);
+
+    public static final class IDs {
+        public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 4);
+        public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 1);
+        public static final CanDevice REAR_LEFT_MOTOR = new CanDevice("SRX_REAR_LEFT", 2);
+        public static final CanDevice REAR_RIGHT_MOTOR = new CanDevice("SRX_REAR_RIGHT", 3);
+        
+        // public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 3);
+        // public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 2);
+        // public static final CanDevice REAR_LEFT_MOTOR = new CanDevice("SRX_REAR_LEFT", 1);
+        // public static final CanDevice REAR_RIGHT_MOTOR = new CanDevice("SRX_REAR_RIGHT", 4);
+    }
+}

src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java

@@ -0,0 +1,118 @@
+package frc4388.robot.subsystems.differential;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status;
+import frc4388.utility.structs.Drivebase;
+
+public class DiffDrive extends SubsystemBase implements Drivebase, Queryable {
+    DiffIO io;
+    DiffStateAutoLogged state = new DiffStateAutoLogged();
+    GyroIO gyroIO;
+    GyroStateAutoLogged gyroState = new GyroStateAutoLogged();
+
+
+    DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(DiffConstants.TRACK_WIDTH);
+    DifferentialDrivePoseEstimator odometry = new DifferentialDrivePoseEstimator(kinematics, new Rotation2d(), 0, 0, new Pose2d());
+
+    public DiffDrive(DiffIO io, GyroIO gyroIO) {
+        this.io = io;
+        this.gyroIO = gyroIO;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(state);
+        Logger.processInputs(getName(), state);
+        gyroIO.updateInputs(gyroState);
+        Logger.processInputs("gyro", gyroState);
+
+        var speeds = new DifferentialDriveWheelPositions(
+            DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.leftOutputPosition),
+            DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.rightOutputPosition)
+        );
+
+        odometry.update(gyroState.yaw, speeds);
+    }
+
+    @Override
+    public void arcadeDrive(Translation2d left, Translation2d right) {
+        io.driveWithInput(
+            left.getY() + right.getX(), 
+            left.getY() - right.getX()
+        );
+    }
+
+    private PID rotPid = new PID(DiffConstants.ROT_GAINS);
+
+    @Override
+    public void driveFieldRelative(Translation2d left, Translation2d right) {
+        // In case the driver's stick is inside the deadband
+        if(right.getNorm() == 0) {
+            io.driveWithInput(0, 0);
+            return;
+        };
+
+        double targetAngle = right.getAngle().getRotations();
+        double curAngle = gyroState.yaw.getRotations();
+
+        double error = targetAngle - curAngle - signedFloor(curAngle);
+        if(error > 0.5) {
+            error -= 1;
+        }else if(error < -0.5) {
+            error += 1;
+        }
+
+        double move = 0;
+        double rot = rotPid.update(targetAngle - curAngle);
+
+        io.driveWithInput(
+            move + rot, 
+            move - rot
+        );
+    }
+
+    private double signedFloor(double x){
+        if(x > 0) {
+            return x - Math.floor(x);
+        } else {
+            return Math.ceil(x) - x;
+        }
+    }
+
+    @Override
+    public void resetOdometry() {
+        gyroIO.reset();
+        odometry.resetPose(new Pose2d());
+    }
+
+
+
+    @AutoLogOutput
+    public Pose2d estimatedPose() {
+        return odometry.getEstimatedPosition();
+    }
+
+
+    @Override
+    public String getName() {
+        return "Diff drive";
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        return new Status();
+    }
+
+}

src/main/java/frc4388/robot/subsystems/differential/DiffIO.java

@@ -0,0 +1,20 @@
+package frc4388.robot.subsystems.differential;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface DiffIO {
+    @AutoLog
+    public class DiffState {
+        public double leftOutputPosition = 0;
+        public double leftOutputVelocity = 0;
+        public double[] leftCurrent = new double[] {};
+
+        public double rightOutputPosition = 0;
+        public double rightOutputVelocity = 0;
+        public double[] rightCurrent = new double[] {};
+    }
+
+    public default void driveWithInput(double leftInput, double rightInput) {}
+
+    public default void updateInputs(DiffState state) {}
+}

src/main/java/frc4388/robot/subsystems/differential/DiffTalonSRX.java

@@ -0,0 +1,52 @@
+package frc4388.robot.subsystems.differential;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
+
+public class DiffTalonSRX implements DiffIO {
+    TalonSRX m_leftFront;
+    TalonSRX m_rightFront;
+    TalonSRX m_leftRear;
+    TalonSRX m_rightRear;
+
+    public DiffTalonSRX(TalonSRX m_leftFront, TalonSRX m_rightFront, TalonSRX m_leftRear, TalonSRX m_rightRear) {
+        this.m_leftFront = m_leftFront;
+        this.m_rightFront = m_rightFront;
+        this.m_leftRear = m_leftRear;
+        this.m_rightRear = m_rightRear;
+
+
+        m_leftRear.follow(m_leftFront);
+        m_rightRear.follow(m_rightFront);
+
+        
+        this.m_rightFront.setInverted(true);
+        this.m_rightRear.setInverted(true);
+    }
+
+    @Override
+    public void updateInputs(DiffState state) {
+        state.leftCurrent = new double[] {m_leftFront.getStatorCurrent(), m_leftRear.getStatorCurrent()};
+        state.leftOutputPosition = (m_leftFront.getSelectedSensorPosition() + m_leftRear.getSelectedSensorPosition()) / 2 / 1100;
+        state.leftOutputVelocity = (m_leftFront.getSelectedSensorVelocity() + m_leftRear.getSelectedSensorVelocity()) / 2 / 1100;
+
+        state.rightCurrent = new double[] {m_rightFront.getStatorCurrent(), m_rightRear.getStatorCurrent()};
+        state.rightOutputPosition = (m_rightFront.getSelectedSensorPosition() + m_rightFront.getSelectedSensorPosition()) / 2 / 1100;
+        state.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.getSelectedSensorVelocity()) / 2 / 1100;
+    }
+    
+    @Override
+    public void driveWithInput(double leftInput, double rightInput) {
+        m_leftFront.set(TalonSRXControlMode.PercentOutput, leftInput);
+        m_rightFront.set(TalonSRXControlMode.PercentOutput, rightInput);
+    }
+
+    @AutoLogOutput
+    public double test() {
+        return m_leftFront.getSelectedSensorPosition();
+    }
+}

src/main/java/frc4388/robot/subsystems/differential/GyroIO.java

@@ -0,0 +1,19 @@
+package frc4388.robot.subsystems.differential;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Voltage;
+
+public interface GyroIO {
+    @AutoLog
+    public class GyroState {
+        public boolean connected = false;
+        public Voltage voltage;
+        public Rotation2d yaw;
+    }
+    
+    public default void updateInputs(GyroState inputs) {}
+
+    public default void reset() {}
+}

src/main/java/frc4388/robot/subsystems/differential/GyroPigeon2.java

@@ -0,0 +1,36 @@
+package frc4388.robot.subsystems.differential;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import frc4388.utility.status.Alerts;
+
+public class GyroPigeon2 implements GyroIO {
+    Pigeon2 m_gyro;
+
+    StatusSignal<Voltage> voltage;
+
+    public GyroPigeon2(Pigeon2 m_gyro){
+        this.m_gyro = m_gyro;
+
+        voltage = m_gyro.getSupplyVoltage();
+    }
+
+    @Override
+    public void updateInputs(GyroState state) {
+        voltage.refresh();
+        
+        state.connected = m_gyro.isConnected();
+        state.voltage = voltage.getValue();
+        state.yaw = m_gyro.getRotation2d();
+
+        Alerts.validate(!state.connected, "Gyro is not connected!", AlertType.kError);
+    }
+
+    @Override
+    public void reset() {
+        m_gyro.reset();
+    }
+}

src/main/java/frc4388/robot/subsystems/differential/PID.java

@@ -0,0 +1,29 @@
+package frc4388.robot.subsystems.differential;
+
+import frc4388.utility.structs.Gains;
+
+public class PID {
+    protected Gains  gains;
+    private   double output    = 0;
+
+
+    /** Creates a new PelvicInflammatoryDisease. */
+    public PID(Gains gains) {
+        this.gains = gains;
+    }
+
+    private double prevError, cumError = 0;
+    
+    // Called every time the scheduler runs while the command is scheduled.
+    public double update(double error) {
+        cumError += error * .02; // 20 ms
+        double delta = error - prevError;
+
+        output = error * gains.kP;
+        output += cumError * gains.kI;
+        output += delta * gains.kD;
+        output += gains.kF;
+
+        return output;
+    }
+}

src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java

@@ -0,0 +1,55 @@
+package frc4388.robot.subsystems.lidar;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.LiDARConstants;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status.ReportLevel;
+
+public class LiDAR extends SubsystemBase implements Queryable {
+    LidarIO io;
+    LidarStateAutoLogged state = new LidarStateAutoLogged();
+
+    private String name = "Lidar";
+
+    public LiDAR(LidarIO device, String name) {
+        FaultReporter.register(this);
+
+        this.io = device;
+        this.name = name;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(state);
+    }
+
+    // @AutoLogOutput(key = "Lidar/{name}")
+    public double getDistance(){
+        return state.distance;
+    }
+
+    public boolean withinDistance(){
+        if(state.distance == -1) return false;
+        return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
+    }
+
+    @Override
+    public String getName() {
+        return "Lidar " + name;
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+
+        if(state.distance == -1)
+            s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
+        
+        s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance);
+
+        return s;
+    }
+    
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java

@@ -0,0 +1,13 @@
+package frc4388.robot.subsystems.lidar;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface LidarIO {
+    @AutoLog
+    public class LidarState {
+        public boolean connected;
+        public double distance;
+    }
+    
+    public default void updateInputs(LidarState state) {}
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java

@@ -0,0 +1,27 @@
+package frc4388.robot.subsystems.lidar;
+
+import edu.wpi.first.wpilibj.Counter;
+import frc4388.robot.constants.Constants.LiDARConstants;
+
+// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
+public class LidarLiteV2 implements LidarIO {
+
+    
+    private Counter LidarPWM;
+
+    public LidarLiteV2(int port) {
+        LidarPWM = new Counter(port);
+        LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
+        LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
+        LidarPWM.reset();
+    }
+
+    @Override
+    public void updateInputs(LidarState state) {
+        
+        if(LidarPWM.get() < 1)
+            state.distance = -1;
+        else
+            state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
+    }
+}

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -0,0 +1,484 @@
+// 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 frc4388.robot.subsystems.swerve;
+
+import java.util.Optional;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+import com.ctre.phoenix6.swerve.SwerveModule;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.subsystems.vision.Vision;
+import frc4388.utility.compute.TimesNegativeOne;
+import frc4388.utility.status.Status;
+import frc4388.utility.structs.Drivebase;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.util.PathPlannerLogging;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.RobotConfig;
+
+public class SwerveDrive extends SubsystemBase implements Queryable, Drivebase {
+    private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+    private Vision vision;
+
+    // @AutoLog
+    // public class SwerveDriveState {
+    public int gear_index = SwerveDriveConstants.STARTING_GEAR;
+    public boolean stopped = false;
+    public boolean robotKnowsWhereItIs = false;
+
+    public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
+    public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
+    public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
+                                                                                            // 25%
+
+    public double lastOdomSpeed;
+
+    public Pose2d initalPose2d = null;
+
+
+    public double rotTarget = 0.0;
+    public Rotation2d orientRotTarget = new Rotation2d();
+    public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+    // }
+
+    // public SwerveDriveState state = new SwerveDriveState();
+
+    /** Creates a new SwerveDrive. */
+    public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
+        // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
+        // swerveDriveTrain) {
+        FaultReporter.register(this);
+
+        this.swerveDriveTrain = swerveDriveTrain;
+        this.vision = vision;
+
+        RobotConfig config;
+        try {
+            config = RobotConfig.fromGUISettings();
+        } catch (Exception e) {
+            // Handle exception as needed
+            config = null;
+        }
+        // DoubleSupplier a = () -> 1.d;
+        AutoBuilder.configure(
+                () -> {
+                    return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
+                }, // Robot pose supplier
+                this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
+                                             // pose)
+                () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
+                        .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
+                                              // Also optionally outputs individual module feedforwards
+                new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
+                                                // holonomic drive trains
+                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+                        new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
+                ),
+                config, // The robot configuration
+                () -> {
+                    // Boolean supplier that controls when the path will be mirrored for the red
+                    // alliance
+                    // This will flip the path being followed to the red side of the field.
+                    // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+                    // var alliance = DriverStation.getAlliance();
+                    // if (alliance.isPresent()) {
+                    //     return alliance.get() == DriverStation.Alliance.Red;
+                    // }
+                    return TimesNegativeOne.isRed;
+                },
+                this // Reference to this subsystem to set requirements
+        );
+
+        PathPlannerLogging.setLogActivePathCallback(
+        (activePath) -> {
+          Logger.recordOutput(
+              "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
+        });
+        
+        PathPlannerLogging.setLogTargetPoseCallback(
+        (targetPose) -> {
+          Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
+        });
+
+
+
+        // // Configure SysId
+        // sysId =
+        //     new SysIdRoutine(
+        //         new SysIdRoutine.Config(
+        //             null,
+        //             null,
+        //             null,
+        //             (state) -> Logger.recordOutput("Drive/SysIdState", toString())),
+        //         new SysIdRoutine.Mechanism(
+        //             (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
+
+    }
+
+    public void setOdoPose(Pose2d pose) {
+        if (pose == null) return;
+        initalPose2d = pose;
+        swerveDriveTrain.resetPose(pose);
+    }
+
+    // public void oneModuleTest(SwerveModule module, Translation2d leftStick,
+    // Translation2d rightStick){
+    // // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
+    // // rightStick.getAngle()
+    // double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) +
+    // Math.pow(leftStick.getY(), 2));
+    // // System.out.println(ang);
+    // // module.go(ang);
+    // // Rotation2d rot = Rotation2d.fromRadians(ang);
+    // Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
+    // SwerveModuleState state = new SwerveModuleState(speed, rot);
+    // module.setDesiredState(state);
+    // }
+
+    public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
+            stopModules(); // stop the swerve
+
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
+            return; // don't bother doing swerve drive math and return early.
+
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+        
+        stopped = false;
+        if (fieldRelative) {
+            
+            leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
+            rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);    
+
+            // ! drift correction
+            if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
+                rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
+                swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
+                    .withVelocityX(leftStick.getX() * speedAdjust)
+                    .withVelocityY(leftStick.getY() * speedAdjust)
+                    .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+                    // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
+                SmartDashboard.putBoolean("drift correction", false);
+            } else {
+                var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+                    .withVelocityX(leftStick.getX() * speedAdjust)
+                    .withVelocityY(leftStick.getY() * speedAdjust)
+                    .withTargetDirection(Rotation2d.fromDegrees(rotTarget));
+                ctrl.HeadingController.setPID(
+                    SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
+                    SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
+                    SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
+                );
+                swerveDriveTrain.setControl(ctrl);
+                SmartDashboard.putBoolean("drift correction", true);
+            }
+
+           
+        } else { // Create robot-relative speeds.
+            swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
+                    .withVelocityX(leftStick.getX() * speedAdjust)
+                    .withVelocityY(-leftStick.getY() * speedAdjust)
+                    .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+        }
+    }
+
+    public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
+        stopped = false;
+        // Create robot-relative speeds.
+        if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
+        swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
+            .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
+            .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
+            .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+        
+    }
+
+
+    public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
+                                                                                               // reason to have a robot
+                                                                                               // relitive version of
+                                                                                               // this, and no pre
+                                                                                               // provided version
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve
+                                                                                           // drive is still going:
+            stopModules(); // stop the swerve
+
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
+            return; // don't bother doing swerve drive math and return early.
+
+        leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+
+        swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+                .withVelocityX(leftStick.getX() * speedAdjust)
+                .withVelocityY(leftStick.getY() * speedAdjust)
+                .withTargetDirection(rightStick.getAngle()));
+    }
+
+    public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+        leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withTargetDirection(heading);
+        ctrl.HeadingController.setPID(
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+        );
+        swerveDriveTrain.setControl(ctrl);
+    }
+
+    public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
+        leftStick = leftStick.rotateBy(heading);
+
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withTargetDirection(heading);
+        // ctrl.HeadingController.setPID(
+        //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+        //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+        //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+        // );
+        swerveDriveTrain.setControl(ctrl);
+    }
+
+    public void setLimits(double limitInAmps) {
+        for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {
+            var talonFXConfigurator = module.getDriveMotor().getConfigurator();
+            var talonFXConfigs = new TalonFXConfiguration();
+
+            talonFXConfigurator.refresh(talonFXConfigs);
+            talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps;
+            talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10;
+            talonFXConfigurator.apply(talonFXConfigs);
+        }
+    }
+
+    public void activateLuigiMode() {
+        setLimits(20);
+    }
+
+    public void deactivateLuigiMode() {
+        setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
+    }
+
+    public boolean rotateToTarget(double angle) {
+        swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+                .withVelocityX(0)
+                .withVelocityY(0)
+                .withTargetDirection(Rotation2d.fromDegrees(angle)));
+
+        if (Math.abs(angle - getGyroAngle()) < 5.0) {
+            return true;
+        }
+
+        return false;
+    }
+
+    public boolean isStopped() {
+        return lastOdomSpeed < AutoConstants.STOP_VELOCITY;
+    }
+
+    public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
+        // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
+        // swerve drive is still going:
+        // stopModules(); // stop the swerve
+
+        // if (leftStick.getNorm() < 0.05) //if no imput
+        // return; // don't bother doing swerve drive math and return early.
+
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+
+        swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+                .withVelocityX(leftStick.getX() * -speedAdjust)
+                .withVelocityY(leftStick.getY() * speedAdjust)
+                .withTargetDirection(rot));
+        // double
+    }
+
+    public double getGyroAngle() {
+        return getPose2d().getRotation().getRadians();
+    }
+
+    public Pose2d getPose2d() {
+        return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
+    }
+
+    @Override
+    public void resetOdometry() {
+        swerveDriveTrain.tareEverything();
+        robotKnowsWhereItIs = false;
+        rotTarget = 0;
+        // vision.resetRotations();
+    }
+
+
+    public void softStop() {
+        stopped = true;
+        swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
+            .withVelocityX(0)
+            .withVelocityY(0)
+            .withRotationalRate(0)
+        ); // stop the modules without breaking
+    }
+
+    public void stopModules() {
+        // stopped = true;
+        // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
+        softStop();
+    }
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run\
+        SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
+        SmartDashboard.putNumber("RotTartget", rotTarget);
+
+        double time = Vision.getTime();
+        double freq =  swerveDriveTrain.getOdometryFrequency();
+
+        Optional<Pose2d> curpose = swerveDriveTrain.samplePoseAt(time);
+        Optional<Pose2d> lastPose = swerveDriveTrain.samplePoseAt(time - freq);
+        
+        vision.setLastOdomPose(curpose);
+        setLastOdomSpeed(curpose, lastPose, freq);
+
+        if (vision.isTag()) {
+            Pose2d pose = vision.getPose2d();
+            if (!robotKnowsWhereItIs) {
+                robotKnowsWhereItIs = true;
+                Pose2d curPose = getPose2d();
+                rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
+            }
+
+            vision.addVisionMeasurement(swerveDriveTrain);
+        }
+
+        // if(e.isPresent())
+    }
+
+    private void reset_index() {
+        gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
+                                                         // robot start in?)
+    }
+
+    public void shiftDown() {
+        if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
+            reset_index(); // If outof bounds: reset index
+        int i = gear_index - 1;
+        if (i == -1)
+            i = 0;
+        setPercentOutput(SwerveDriveConstants.GEARS[i]);
+        gear_index = i;
+    }
+
+    public void shiftUp() {
+        if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
+            reset_index(); // If outof bounds: reset index
+        int i = gear_index + 1;
+        if (i == SwerveDriveConstants.GEARS.length)
+            i = SwerveDriveConstants.GEARS.length - 1;
+        setPercentOutput(SwerveDriveConstants.GEARS[i]);
+        gear_index = i;
+    }
+
+    public void setPercentOutput(double speed) {
+        speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
+        gear_index = -1;
+    }
+
+    public void setToSlow() {
+        setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
+        gear_index = 0;
+    }
+
+    public void setToFast() {
+        setPercentOutput(SwerveDriveConstants.FAST_SPEED);
+        gear_index = 1;
+    }
+
+    public void setToTurbo() {
+        setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
+        gear_index = 2;
+    }
+
+    public void shiftUpRot() {
+        rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
+    }
+
+    public void shiftDownRot() {
+        rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
+    }
+
+    private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
+
+    public void startSlowPeriod() {
+        tmp_gear_index = gear_index;
+        setToSlow();
+    }
+
+    public void startTurboPeriod() {
+        tmp_gear_index = gear_index;
+        setToTurbo();
+    }
+
+    public void endSlowPeriod() {
+        setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
+        gear_index = tmp_gear_index;
+    }
+
+
+
+    public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
+        if(curPose.isPresent() && lastPose.isPresent()){
+            lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
+        }
+    }
+    
+
+
+    @Override
+    public String getName() {
+        return "Swerve Drive Controller";
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status status = new Status();
+
+        // status.addReport(ReportLevel.INFO,
+        //         "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
+
+        return status;
+    }
+
+
+    // Update CTRE simulation, if used.
+    public void updateSim(double voltage) {
+        swerveDriveTrain.updateSimState(0.02, voltage);
+    }
+}
+

src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java

@@ -0,0 +1,246 @@
+package frc4388.robot.subsystems.swerve;
+
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Rotations;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
+import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Distance;
+import frc4388.utility.status.CanDevice;
+import frc4388.utility.structs.Gains;
+
+
+// No mans land
+// Beware, there be dragons.
+public final class SwerveDriveConstants {
+    public static final double MAX_ROT_SPEED        = Math.PI * 2;
+    public static final double AUTO_MAX_ROT_SPEED = 1.5;
+    public static final double MIN_ROT_SPEED        = 1.0;
+    public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
+    public static       double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
+    public static       double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
+
+    public static final double CORRECTION_MIN = 10;
+    public static final double CORRECTION_MAX = 50;
+    
+    public static final double SLOW_SPEED = 0.25;
+    public static final double FAST_SPEED = 0.5;
+    public static final double TURBO_SPEED = 1.0;
+    
+    public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED};
+    public static final int STARTING_GEAR = 0;
+    // Dimensions
+    public static final double WIDTH = 18.5; // TODO: validate
+    public static final double HEIGHT = 18.5; // TODO: validate
+    public static final double HALF_WIDTH = WIDTH / 2.d;
+    public static final double HALF_HEIGHT = HEIGHT / 2.d;
+
+    // Mechanics
+    private static final double COUPLE_RATIO = 3; //todo: find
+    private static final double DRIVE_RATIO = 6.12;
+    private static final double STEER_RATIO = (150/7);
+    private static final Distance WHEEL_RADIUS = Inches.of(2);
+    
+    public static final double MAX_SPEED_MEETERS_PER_SEC = 6.22; // TODO: Validate
+
+    public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084;
+    public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
+
+    // Operation
+    public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
+
+    public static final boolean DRIFT_CORRECTION_ENABLED = true;
+    public static final boolean INVERT_X = false;
+    public static final boolean INVERT_Y = true;
+    public static final boolean INVERT_ROTATION = false;
+
+    // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
+
+    private static final class ModuleSpecificConstants { //2025
+        //Front Left
+        private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
+        private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
+        private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
+        private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
+        private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
+        private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
+        
+        //Front Right
+        private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
+        private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
+        private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
+        private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
+        private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
+        private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
+
+        //Back Left
+        private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5);
+        private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
+        private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
+        private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
+        private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
+        private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
+        
+        //Back Right
+        private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5);
+        private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
+        private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
+        private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
+        private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
+        private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
+    }
+
+    public static final class IDs {
+        public static final CanDevice RIGHT_FRONT_WHEEL   = new CanDevice("RIGHT_FRONT_WHEEL", 4);
+        public static final CanDevice RIGHT_FRONT_STEER   = new CanDevice("RIGHT_FRONT_STEER", 5);
+        public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 11);
+        
+        public static final CanDevice LEFT_FRONT_WHEEL    = new CanDevice("LEFT_FRONT_WHEEL", 2);
+        public static final CanDevice LEFT_FRONT_STEER    = new CanDevice("LEFT_FRONT_STEER", 3);
+        public static final CanDevice LEFT_FRONT_ENCODER  = new CanDevice("LEFT_FRONT_ENCODER", 10);
+        
+        public static final CanDevice LEFT_BACK_WHEEL     = new CanDevice("LEFT_BACK_WHEEL", 6);
+        public static final CanDevice LEFT_BACK_STEER     = new CanDevice("LEFT_BACK_STEER", 7);
+        public static final CanDevice LEFT_BACK_ENCODER   = new CanDevice("LEFT_BACK_ENCODER", 12);
+        
+        public static final CanDevice RIGHT_BACK_WHEEL    = new CanDevice("RIGHT_BACK_WHEEL", 8);  
+        public static final CanDevice RIGHT_BACK_STEER    = new CanDevice("RIGHT_BACK_STEER", 9);
+        public static final CanDevice RIGHT_BACK_ENCODER  = new CanDevice("RIGHT_BACK_ENCODER", 13);
+
+        public static final CanDevice DRIVE_PIGEON        = new CanDevice("DRIVE_PIGEON", 14);
+    }
+
+    public static final class PIDConstants {
+        public static final int SWERVE_SLOT_IDX = 0;
+        public static final int SWERVE_PID_LOOP_IDX = 1;
+        public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0);
+
+        public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs()
+            .withKP(50).withKI(0).withKD(0.32)
+            .withKS(0).withKV(0).withKA(0);
+        
+            public static final Slot0Configs TEST_SWERVE_ROT_GAINS = new Slot0Configs()
+            .withKP(10).withKI(0).withKD(0.3)
+            .withKS(0).withKV(0).withKA(0);
+
+        public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
+
+        // The steer motor uses any SwerveModule.SteerRequestType control request with the
+        // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+        public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs()
+            .withKP(100).withKI(0).withKD(0.6)
+            .withKS(0.1).withKV(1.91).withKA(0)
+            .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+        // When using closed-loop control, the drive motor uses the control
+        // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+        public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs()
+            .withKP(0.1).withKI(0).withKD(0)
+            .withKS(0).withKV(0.124);
+        
+        public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
+        public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
+    }
+
+    public static final class Configurations {
+        public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
+        public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
+        public static final double NEUTRAL_DEADBAND = 0.04;
+
+        // POWER! (limiting)
+        private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration()
+            .withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Brake)
+                    .withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND)
+            ).withOpenLoopRamps(
+                new OpenLoopRampsConfigs()
+                    .withDutyCycleOpenLoopRampPeriod(OPEN_LOOP_RAMP_RATE)
+            ).withClosedLoopRamps(
+                new ClosedLoopRampsConfigs()
+                    .withDutyCycleClosedLoopRampPeriod(CLOSED_LOOP_RAMP_RATE)
+        );
+        private static final TalonFXConfiguration STEER_MOTOR_CONFIGS = new TalonFXConfiguration()
+            .withCurrentLimits(
+                new CurrentLimitsConfigs()
+                    .withStatorCurrentLimit(40) // TODO: tune???
+                    .withStatorCurrentLimitEnable(true)
+                ).withMotorOutput(
+                    new MotorOutputConfigs()
+                        .withNeutralMode(NeutralModeValue.Brake)
+                        .withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND)
+                // ).withOpenLoopRamps(
+                //     new OpenLoopRampsConfigs()
+                //         .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
+                // ).withClosedLoopRamps(
+                //     new ClosedLoopRampsConfigs()
+                //         .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
+        );
+        public static final double SLIP_CURRENT = 60; // TODO: Tune??? 
+    }
+
+    public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
+        .withPigeon2Id(IDs.DRIVE_PIGEON.id);
+
+    private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
+        new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // holy verbosity batman.
+            .withDriveMotorGearRatio(DRIVE_RATIO)
+            .withSteerMotorGearRatio(STEER_RATIO)
+            .withCouplingGearRatio(COUPLE_RATIO)
+            .withWheelRadius(WHEEL_RADIUS)
+            .withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ?
+            .withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ?
+            .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage)
+            .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage)
+            .withSlipCurrent(Configurations.SLIP_CURRENT)
+            .withSpeedAt12Volts(MAX_SPEED_MEETERS_PER_SEC)
+            .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated)
+            .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated)
+            .withFeedbackSource(SteerFeedbackType.RemoteCANcoder)
+            .withDriveMotorInitialConfigs(Configurations.DRIVE_MOTOR_CONFIGS)
+            .withSteerMotorInitialConfigs(Configurations.STEER_MOTOR_CONFIGS);
+
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_LEFT =
+        ConstantCreator.createModuleConstants(
+            IDs.LEFT_FRONT_STEER.id, IDs.LEFT_FRONT_WHEEL.id, IDs.LEFT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_LEFT_ENCODER_OFFSET,
+            ModuleSpecificConstants.FRONT_LEFT_XPOS, ModuleSpecificConstants.FRONT_LEFT_YPOS, 
+            ModuleSpecificConstants.FRONT_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_ENCODER_INVERTED
+    );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_RIGHT =
+        ConstantCreator.createModuleConstants(
+            IDs.RIGHT_FRONT_STEER.id, IDs.RIGHT_FRONT_WHEEL.id, IDs.RIGHT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_OFFSET,
+            ModuleSpecificConstants.FRONT_RIGHT_XPOS, ModuleSpecificConstants.FRONT_RIGHT_YPOS, 
+            ModuleSpecificConstants.FRONT_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_INVERTED
+    );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_LEFT =
+        ConstantCreator.createModuleConstants(
+            IDs.LEFT_BACK_STEER.id, IDs.LEFT_BACK_WHEEL.id, IDs.LEFT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_LEFT_ENCODER_OFFSET,
+            ModuleSpecificConstants.BACK_LEFT_XPOS, ModuleSpecificConstants.BACK_LEFT_YPOS, 
+            ModuleSpecificConstants.BACK_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_ENCODER_INVERTED
+    );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_RIGHT =
+        ConstantCreator.createModuleConstants(
+            IDs.RIGHT_BACK_STEER.id, IDs.RIGHT_BACK_WHEEL.id, IDs.RIGHT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_RIGHT_ENCODER_OFFSET,
+            ModuleSpecificConstants.BACK_RIGHT_XPOS, ModuleSpecificConstants.BACK_RIGHT_YPOS, 
+            ModuleSpecificConstants.BACK_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_ENCODER_INVERTED
+    );
+    
+    // misc
+    public static final int TIMEOUT_MS = 30;
+    public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+}

src/main/java/frc4388/robot/subsystems/vision/Vision.java

@@ -0,0 +1,94 @@
+package frc4388.robot.subsystems.vision;
+
+import java.util.Optional;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status;
+
+public class Vision extends SubsystemBase implements Queryable {
+    VisionIO[] io;
+    VisionStateAutoLogged[] state;
+
+    
+    public Pose2d lastVisionPose = new Pose2d();
+    public Pose2d lastPhysOdomPose = new Pose2d();
+
+    public Vision(VisionIO... devices) {
+        FaultReporter.register(this);
+        io = devices;
+        state = new VisionStateAutoLogged[io.length];
+
+        for(int i = 0; i < io.length; i++) {
+            state[i] = new VisionStateAutoLogged();
+        }
+    }
+
+    @Override
+    public void periodic() {
+        for(int i = 0; i < io.length; i++) {
+            io[i].updateInputs(state[i]);
+            Logger.processInputs("Vision/Camera" + i , state[i]);
+        }
+    }
+
+    public void addVisionMeasurement(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
+        // for(EstimatedRobotPose pose : poses){
+        //     
+        // }
+        for(int i = 0; i < state.length; i++) {
+            if(state[i].lastEstimatedPose != null) {
+                PoseObservation pose = state[i].lastEstimatedPose;
+                drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
+            }
+        }
+    }
+
+    public void setLastOdomPose(Optional<Pose2d> pose){
+        if(pose.isPresent())
+            lastPhysOdomPose = pose.get();
+    }
+
+    public boolean isTag(){
+        for(int i = 0; i < state.length; i++){
+            if(state[i].isTagDetected && state[i].isTagProcessed)
+                return true;
+        }
+        return false;
+    }
+
+    @AutoLogOutput
+    public Pose2d getPose2d() {
+        if(lastPhysOdomPose != null)
+            return lastPhysOdomPose;
+
+        // if(lastVisionPose != null)
+        //     return lastVisionPose;
+        return new Pose2d();
+
+    }
+
+    public static double getTime() {
+        return Utils.getCurrentTimeSeconds();
+    }
+
+
+    @Override
+    public Status diagnosticStatus() {
+        return new Status();
+        // // TODO Auto-generated method stub
+        // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'");
+    }
+    
+}

src/main/java/frc4388/robot/subsystems/vision/VisionIO.java

@@ -0,0 +1,22 @@
+package frc4388.robot.subsystems.vision;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public interface VisionIO {
+    @AutoLog
+    public class VisionState {
+        public boolean isTagDetected = false;
+        public boolean isTagProcessed = false;
+        // public double latency = 0;
+        public PoseObservation lastEstimatedPose = null;
+    }
+
+    public static record PoseObservation(
+        Pose2d pose,
+        double timestamp
+    ) {}
+
+    public default void updateInputs(VisionState state) {}
+}

src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java

@@ -0,0 +1,158 @@
+package frc4388.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+import frc4388.robot.constants.Constants.FieldConstants;
+import frc4388.robot.constants.Constants.VisionConstants;
+
+public class VisionPhotonvision implements VisionIO {
+    // private PhotonCamera[] cameras;
+    // private PhotonPoseEstimator[] estimators;
+    
+    private PhotonCamera camera;
+    private PhotonPoseEstimator estimator;
+
+    // public List<EstimatedRobotPose> poses = new ArrayList<>();
+
+    
+    public VisionPhotonvision(PhotonCamera camera, Transform3d position){
+        this.camera = camera;
+        estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position);
+        estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+    }
+
+    // private Instant lastVisionTime = null;
+
+
+    public void updateInputs(VisionState state) {
+        state.isTagProcessed = false;
+        state.isTagDetected = false;
+
+        state.lastEstimatedPose = null;
+
+        var results = camera.getAllUnreadResults();
+
+        // If there are no more updates from the camera
+        if (results.size() == 0) 
+            return;
+
+        
+        var result = results.get(results.size()-1);
+
+        state.isTagDetected = state.isTagDetected | result.hasTargets();
+
+        // If there are no tags
+        if(!result.hasTargets())
+            return;
+
+        Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
+
+        // If the tag was failed to be processed
+        if(estimatedRobotPose.isEmpty())
+            return;
+        
+        EstimatedRobotPose pose = estimatedRobotPose.get();
+
+        state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds);
+
+        state.isTagProcessed = true;
+    }
+
+
+    /**
+     * The latest estimated robot pose on the field from vision data. This may be empty. This should
+     * only be called once per loop.
+     *
+     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
+     * {@link getEstimationStdDevs}
+     *
+     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
+     *     used for estimation.
+     */
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) {
+        Optional<EstimatedRobotPose> visionEst = Optional.empty();
+
+        var targets = change.getTargets();
+        for(int i = targets.size()-1; i >= 0; i--){
+            Transform3d pos = targets.get(i).getBestCameraToTarget();
+            double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
+            if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
+                change.targets.remove(i);
+            }
+        }
+
+        if(targets.size() <= 0)
+            return visionEst; // Will be empty
+
+        visionEst = estimator.update(change);
+        // updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
+
+        return visionEst;
+    }
+
+    public String getName() {
+        return camera.getName();
+    }
+
+
+    // /**
+    //  * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
+    //  * deviations based on number of tags, estimation strategy, and distance from the tags.
+    //  *
+    //  * @param estimatedPose The estimated pose to guess standard deviations for.
+    //  * @param targets All targets in this camera frame
+    //  */
+    // private void updateEstimationStdDevs(
+    //         Optional<EstimatedRobotPose> estimatedPose, 
+    //         List<PhotonTrackedTarget> targets,
+    //         PhotonPoseEstimator estimator) {
+    //     if (estimatedPose.isEmpty()) {
+    //         // No pose input. Default to single-tag std devs
+    //         curStdDevs = VisionConstants.kSingleTagStdDevs;
+
+    //     } else {
+    //         // Pose present. Start running Heuristic
+    //         var estStdDevs = VisionConstants.kSingleTagStdDevs;
+    //         int numTags = 0;
+    //         double avgDist = 0;
+
+    //         // Precalculation - see how many tags we found, and calculate an average-distance metric
+    //         for (var tgt : targets) {
+    //             var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
+    //             if (tagPose.isEmpty()) continue;
+                
+    //             double distance = tagPose
+    //             .get()
+    //             .toPose2d()
+    //             .getTranslation()
+    //             .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+                
+    //             if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
+    //                 numTags++;
+    //                 avgDist += distance;
+    //             }
+    //         }
+
+    //         if (numTags == 0) {
+    //             // No tags visible. Default to single-tag std devs
+    //             curStdDevs = VisionConstants.kSingleTagStdDevs;
+    //         } else {
+    //             // One or more tags visible, run the full heuristic.
+    //             avgDist /= numTags;
+    //             // Decrease std devs if multiple targets are visible
+    //             if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
+    //             // Increase std devs based on (average) distance
+    //             if (numTags == 1 && avgDist > 4)
+    //                 estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    //             else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+    //             curStdDevs = estStdDevs;
+    //         }
+    //     }
+    // }
+}

src/main/java/frc4388/utility/AprilTag.java

@@ -1,13 +0,0 @@
-package frc4388.utility;
-
-// This is a seperate class in case I want to encode rotation or other
-// information about the tag
-public class AprilTag {
-    public final double x, y, z;
-
-    public AprilTag(double _x, double _y, double _z) {
-        x = _x;
-        y = _y;
-        z = _z;
-    }
-}

src/main/java/frc4388/utility/DeferredBlock.java

@@ -2,22 +2,40 @@ package frc4388.utility;
 
 import java.util.ArrayList;
 
+// Class for running code snippets whenever the robot is enabled.
 public class DeferredBlock {
-    private static ArrayList<Runnable> m_blocks = new ArrayList<>();
+    private static ArrayList<Runnable> m_blocks_norerun = new ArrayList<>();
+    private static ArrayList<Runnable> m_blocks_rerun = new ArrayList<>();
     private static boolean             m_hasRun = false;
 
-    public DeferredBlock(Runnable block) {
-        m_blocks.add(block);
+    public static void addBlock(Runnable block) {
+        addBlock(block, false);
+    }
+
+
+    public static void addBlock(Runnable block, boolean rerun) {
+        if(rerun) {
+            m_blocks_rerun.add(block);
+        } else {
+            m_blocks_norerun.add(block);
+        }
     }
 
     public static void execute() {
-        if (m_hasRun) return;
 
-        for (Runnable block : m_blocks) {
+        // Run blocks that run multiple times.
+        for (Runnable block : m_blocks_rerun) {
             block.run();
         }
 
-        m_blocks.clear(); // for garbage collection
+        // Run blocks that only run once
+        if (m_hasRun) return;
+
+        for (Runnable block : m_blocks_norerun) {
+            block.run();
+        }
+
+        m_blocks_norerun.clear(); // for garbage collection
         m_hasRun = true;
     }
 }

src/main/java/frc4388/utility/RobotGyro.java

@@ -1,294 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.utility;
-
-// import com.ctre.phoenix.sensors.WPI_Pigeon2;
-import com.ctre.phoenix6.hardware.Pigeon2;
-// import com.kauailabs.navx.frc.AHRS;
-
-// import edu.wpi.first.wpilibj.GyroBase;
-// import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-
-/**
- * Gyro class that allows for interchangeable use between a pigeon and a navX
- */
-public class RobotGyro {
-    private RobotTime m_robotTime = RobotTime.getInstance();
-
-    private Pigeon2 m_pigeon = null;
-    // private AHRS m_navX = null;
-    public boolean m_isGyroAPigeon; //true if pigeon, false if navX
-
-    private double m_lastPigeonAngle;
-    private double m_deltaPigeonAngle;
-
-    private double pitchZero = 0;
-    private double rollZero = 0;
-
-    /**
-     * Creates a Gyro based on a pigeon
-     * @param gyro the gyroscope to use for Gyro
-     */
-    public RobotGyro(Pigeon2 gyro) {
-        m_pigeon = gyro;
-        m_isGyroAPigeon = true;
-    }
-
-    /**
-     * Creates a Gyro based on a navX
-     * @param gyro the gyroscope to use for Gyro
-     */
-    // public RobotGyro(AHRS gyro){
-    //     m_navX = gyro;
-    //     m_isGyroAPigeon = false;
-    // }
-
-    /**
-     * Resets yaw, pitch, and roll.
-     */
-    public void resetZeroValues() {
-        if (!m_isGyroAPigeon) return;
-
-        // pitchZero = m_pigeon.getPitch();
-        // rollZero =  m_pigeon.getRoll();
-    }
-
-    /**
-     * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
-     * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
-     */
-    public void updatePigeonDeltas() {
-        double currentPigeonAngle = getAngle();
-        m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
-        m_lastPigeonAngle = currentPigeonAngle;
-    }
-
-    /**
-     * <p>NavX:
-     * <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
-     * the center value as the Accumulator center value for subsequent measurements. It's important to
-     * make sure that the robot is not moving while the centering calculations are in progress, this
-     * is typically done when the robot is first turned on while it's sitting at rest before the
-     * competition starts.
-     * 
-     * <p>Pigeon:
-     * <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
-     * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
-     * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
-     * make sure that the robot is not moving while the tempurature calculations are in progress, this
-     * is typically done when the robot is first turned on while it's sitting at rest before the
-     * competition starts.
-     */
-    public void calibrate() {
-        return;
-        // if (m_isGyroAPigeon) {
-        //     m_pigeon.calibrate();
-        // } else {
-        //     m_navX.calibrate();
-        // }
-    }
-
-    public void reset() {
-        resetZeroValues();
-
-        // if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(0);
-        // } else {
-        //     // m_navX.reset();
-        // }
-
-    }
-
-    public void reset(double val) {
-        resetZeroValues();
-
-        // if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(val);
-        // } else {
-        //     // m_navX.reset();
-        // }
-
-    }
-
-    public void resetFlip() {
-        resetZeroValues();
-
-        // if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(180);
-        // } else {
-            // m_navX.reset();
-        // }
-
-    }
-
-    public void resetNinety() {
-        resetZeroValues();
-        
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(90);
-        } else {
-            // m_navX.reset();
-        }
-        
-    }
-
-    public void resetTwoSeventy() {
-        resetZeroValues();
-
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(270);
-        } else {
-            // m_navX.reset();
-        }
-
-    }
-
-    public void resetRightSideBlue() {
-        resetZeroValues();
-
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(60);
-        } else {
-            // m_navX.reset();
-        }   
-
-    }
-
-    public void resetAmpSide() {
-        resetZeroValues();
-
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(-60);
-        } else {
-            // m_navX.reset();
-        }   
-
-    }
-
-    /**
-	 * Get Yaw, Pitch, and Roll data.
-	 *
-	 * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
-	 *					Yaw is within [-368,640, +368,640] degrees.
-	 *					Pitch is within [-90,+90] degrees.
-	 *					Roll is within [-90,+90] degrees.
-     */
-    private double[] getPigeonAngles() {
-        m_pigeon.getAngle();
-        var rotation = m_pigeon.getRotation3d();
-
-        return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
-    }
-
-    public Rotation2d getRotation2d() {
-        return m_pigeon.getRotation2d();
-    }
-
-    public Rotation3d getRotation3d() {
-        return m_pigeon.getRotation3d();
-    }
-
-    public Translation2d getAcceleration2d() {
-        return new Translation2d(
-            m_pigeon.getAccelerationX().getValue().baseUnitMagnitude(), 
-            m_pigeon.getAccelerationY().getValue().baseUnitMagnitude());
-    }
-
-
-    public Translation3d getAcceleration3d() {
-        return new Translation3d(
-            m_pigeon.getAccelerationX().getValue().baseUnitMagnitude(), 
-            m_pigeon.getAccelerationY().getValue().baseUnitMagnitude(), 
-            m_pigeon.getAccelerationZ().getValue().baseUnitMagnitude());
-    }
-    
-    public double getAngularVelocity() {
-        return m_pigeon.getAngularVelocityYDevice().getValueAsDouble();
-    }
-
-    public double getAngle() {
-        // if (m_isGyroAPigeon) {
-            return getPigeonAngles()[2];
-        // } else {
-            // return m_navX.getAngle();
-        // }
-    }
-
-    public double getYaw() {
-       return this.getAngle();
-    }
-
-    /**
-     * Gets an absolute heading of the robot
-     * @return heading from -180 to 180 degrees
-     */
-    public double getHeading() {
-        return getHeading(getAngle());
-    }
-
-    /**
-     * Gets an absolute heading of the robot
-     * @return heading from -180 to 180 degrees
-     */
-    public double getHeading(double angle) {
-        return Math.IEEEremainder(angle, 360);
-    }
-
-    /**
-     * Returns the current pitch value (in degrees, from -90 to 90)
-     * reported by the sensor.  Pitch is a measure of rotation around
-     * the Y Axis.
-     * @return The current pitch value in degrees (-90 to 90).
-     */
-    public double getPitch() {
-        // if (m_isGyroAPigeon) {
-            return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
-        // } else {
-            // return MathUtil.clamp(m_navX.getPitch(), -90, 90);
-        // }
-    }
-
-    /**
-     * Returns the current roll value (in degrees, from -90 to 90)
-     * reported by the sensor.  Roll is a measure of rotation around
-     * the X Axis.
-     * @return The current roll value in degrees (-90 to 90).
-     */
-    public double getRoll() {
-        // if (m_isGyroAPigeon) {
-            return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
-        // } else {
-            // return MathUtil.clamp(m_navX.getRoll(), -90, 90);
-        // }
-    }
-
-    public double getRate() {
-        // if (m_isGyroAPigeon) {
-            return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
-        // } else {
-        //     // return m_navX.getRate();
-        // }
-    }
-
-    public Pigeon2 getPigeon(){
-        return m_pigeon;
-    }
-
-    // public AHRS getNavX(){
-    //     return m_navX;
-    // }
-
-    public void close() throws Exception {
-
-    }
-}

src/main/java/frc4388/utility/compute/DataUtils.java

@@ -1,4 +1,4 @@
-package frc4388.utility;
+package frc4388.utility.compute;
 
 import java.nio.ByteBuffer;
 

src/main/java/frc4388/utility/compute/ReefPositionHelper.java

@@ -0,0 +1,106 @@
+package frc4388.utility.compute;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.constants.Constants.FieldConstants;
+
+public class ReefPositionHelper {
+    public enum Side {
+        LEFT,
+        RIGHT,
+        CENTER,
+        FAR_LEFT
+    }
+
+    public static final Pose2d[] RED_TAGS = {
+        FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(11).get().toPose2d()
+    };
+
+    public static final Pose2d[] BLUE_TAGS = {
+        FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(22).get().toPose2d()
+    };
+
+    public static double distanceTo(Pose2d first, Pose2d second){
+        return Math.sqrt(Math.pow(first.getX()  -  second.getX(),2) + Math.pow(first.getY()  -  second.getY(),2));
+    }
+
+
+    /*
+    * Function to loop through a list of tag locations to figure out closest one
+    */
+    public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){
+        if(locations.length <= 0) return new Pose2d();
+
+        Pose2d minPos = locations[0];
+        double minDistance = distanceTo(position,minPos);
+
+        for(int i = 1; i < locations.length; i++){
+            double dist = distanceTo(locations[i],position);
+            if(dist < minDistance){
+                minPos = locations[i];
+                minDistance = dist;
+            }
+        }
+        
+        System.out.println(minPos.getRotation().getDegrees());
+
+        return minPos;
+    }
+
+    /*
+     * Function to find closest tag location based on side
+     */
+    public static Pose2d getNearestTag(Pose2d position) {
+
+        if(TimesNegativeOne.isRed)
+            return getNearestTag(RED_TAGS, position);
+        else
+            return getNearestTag(BLUE_TAGS, position);
+    }
+
+    public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
+        return offset(getNearestTag(position), 
+        getSide(side) + xtrim,
+        ydistance); 
+    }
+
+    public static double getSide(Side side){
+        switch(side) {
+            case LEFT:
+                return -(AutoConstants.X_SCORING_POSITION_OFFSET);
+            case FAR_LEFT:
+                return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8));
+            case RIGHT:
+                return (AutoConstants.X_SCORING_POSITION_OFFSET);
+            case CENTER:
+                return 0;
+        }
+        assert false;
+        return 0;
+    }
+
+
+    public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
+        Translation2d oldTranslation = oldPose.getTranslation();
+        
+        double rot = oldPose.getRotation().getRadians();
+
+        return new Pose2d(new Translation2d(
+            oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset,
+            oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
+        ), oldPose.getRotation().rotateBy(Rotation2d.k180deg));
+    }
+}

src/main/java/frc4388/utility/compute/RobotTime.java

@@ -5,7 +5,7 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc4388.utility;
+package frc4388.utility.compute;
 
 /**
  * <p>Keeps track of Robot times like time passed, delta time, etc