Add 2026 code

.wpilib/wpilib_preferences.json

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

build.gradle

@@ -1,6 +1,7 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.3.2"
+    id "edu.wpi.first.GradleRIO" version "2026.2.1"
+    id "com.peterabeles.gversion" version "1.10"
 }
 
 java {
@@ -33,6 +34,8 @@ deploy {
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
+                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
+                                           // longer exist in deploy directory of this project
                 }
             }
         }
@@ -41,15 +44,17 @@ deploy {
 
 def deployArtifact = deploy.targets.roborio.artifacts.frcJava
 
-// Set to true to use debug for JNI.
+// Set to true to use debug for all targets including JNI, which will drastically impact
+// performance.
 wpi.java.debugJni = false
 
 // Set this to true to enable desktop support.
-def includeDesktopSupport = false
+def includeDesktopSupport = true
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.
 dependencies {
+    annotationProcessor wpi.java.deps.wpilibAnnotations()
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 
@@ -67,14 +72,20 @@ dependencies {
     nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
     simulationRelease wpi.sim.enableRelease()
 
-    //testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    //testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    
+    implementation("com.fazecast:jSerialComm:2.4.1")
+
+        def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }
 
-// test {
+test {
-//     useJUnitPlatform()
+    useJUnitPlatform()
-//     systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
-//}
+}
 
 // Simulation configuration (e.g. environment variables).
 wpi.sim.addGui().defaultEnabled = true
@@ -85,7 +96,9 @@ wpi.sim.addDriverstation()
 // knows where to look for our Robot Class.
 jar {
     from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
-    from sourceSets.main.allSource
+    from('src') { into 'backup/src' }
+    from('vendordeps') { into 'backup/vendordeps' }
+    from('build.gradle') { into 'backup' }
     manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
     duplicatesStrategy = DuplicatesStrategy.INCLUDE
 }
@@ -98,4 +111,20 @@ wpi.java.configureTestTasks(test)
 // Configure string concat to always inline compile
 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       = "  "
 }

settings.gradle

@@ -4,7 +4,7 @@ pluginManagement {
     repositories {
         mavenLocal()
         gradlePluginPortal()
-        String frcYear = '2024'
+        String frcYear = '2026'
         File frcHome
         if (OperatingSystem.current().isWindows()) {
             String publicFolder = System.getenv('PUBLIC')
@@ -20,8 +20,8 @@ pluginManagement {
         }
         def frcHomeMaven = new File(frcHome, 'maven')
         maven {
-            name 'frcHome'
+            name = 'frcHome'
-            url frcHomeMaven
+            url = frcHomeMaven
         }
     }
 }

src/main/deploy/pathplanner/autos/Center-Shoot.auto

@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center-Hub-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto

@@ -0,0 +1,44 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Robot Shoot"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Shoot driving across"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Robot Shoot Driving"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Spring Break Tests",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/HubCenter-Score-PushLeft.auto

@@ -0,0 +1,67 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center-Hub-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center-NeutralL"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarLeft-NeutralZone 1-2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Retracted"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarLeft-NeutralZone 2-2"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/HubCenter-Score-PushRight.auto

@@ -0,0 +1,67 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center-Hub-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center-NeutralR"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarRight-NeutralZone 1-2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Retracted"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarRight-NeutralZone 2-2"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/HubCenter-Station-Score.auto

@@ -0,0 +1,56 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "HubCenter-PlayerStation"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CPlayerStation-CShoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/HubFarLeft-Score-Push.auto

@@ -0,0 +1,62 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.15
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarLeft-NeutralZone 1-2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarLeft-NeutralZone 2-2"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "HubFarRight-Shoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Robot Rev Up"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/HubFarRight-Score-Push.auto

@@ -0,0 +1,62 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.5
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Copy of HubFarRight-NeutralZone 1-2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarRight-NeutralZone 2-2"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "HubFarLeft-Shoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Robot Rev Up"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/HubLeft-Score.auto

@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubLeft-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/HubRight-Score.auto

@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubRight-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/HubRight-Station-Score-Push.auto

@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "HubRight-PlayerStation"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "PlayerStation-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "ReadyPush"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarRight-NeutralZone 1-2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarRight-NeutralZone 2-2"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/HubRight-Station-Score.auto

@@ -0,0 +1,56 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "HubRight-PlayerStation"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "PlayerStation-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Pikes Peak Autos",
+  "choreoAuto": false
+}

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

@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bump test"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BumpToCenter"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarLeft-NeutralZone 2-2"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Spring Break Tests",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Test-LClimb.auto

@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Climber Extended"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.2
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftTower"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.2
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Climber Retracted"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Spring Break Tests",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Test-RClimb.auto

@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Climber Extended"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.2
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightTower"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.2
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Climber Retracted"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Spring Break Tests",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot-Neutral-Shoot.auto

@@ -0,0 +1,123 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LeftBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftBump-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LeftBump2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftBump2-Neutral2-Shoot2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot.auto

@@ -0,0 +1,68 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LeftBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftBump-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto

@@ -0,0 +1,55 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftTrench-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Trench-Shoot-Neutral-Shoot.auto

@@ -0,0 +1,149 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LeftTrench-Neutral-Trench-Shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Labubu Growl"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 4.2
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Rev Up"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Shoot"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "ShotL-Neutral"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 2.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Labubu Growl"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Rev Up"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Shoot"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Trench-Shoot.auto

@@ -0,0 +1,93 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Zero Encoder"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LeftTrench-Neutral-Trench-Shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Labubu Growl"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 5.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Rev Up"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Shoot"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot-Neutral-Shoot.auto

@@ -0,0 +1,123 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RightBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightBump-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RightBump2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightBump2-Neutral2-Shoot2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot.auto

@@ -0,0 +1,68 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RightBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightBump-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Ramp-Shoot-PlayerStation-Shoot.auto

@@ -0,0 +1,136 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "RightTrench-Neutral-Shoot"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "RightRamp-Shoot"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "BumpOffsetBackward"
+                      }
+                    }
+                  ]
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Labubu Growl"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 2.8499999999999996
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Rev Up"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Shoot"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "ShotRightR-PlayerStation"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "PlayerStation-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto

@@ -0,0 +1,55 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightTrench-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Trench-Shoot-PlayerStation-Shoot.auto

@@ -0,0 +1,123 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Zero Encoder"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RightTrench-Neutral-Trench-Shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Labubu Growl"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 4.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Rev Up"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Robot Shoot"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "ShotRightT-PlayerStation"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.5
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "PlayerStation-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. Sierra.auto

@@ -0,0 +1,99 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Zero Encoder"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "SierraOne"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Labubu Growl"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SierraTwo"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.5
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "PlayerStation-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. zawg.auto

@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Reference"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightTrench-FirstSwipe"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4908076923076936,
+        "y": 6.633345195729537
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.3977564102564113,
+        "y": 6.0868195547038955
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.4908076923076936,
+        "y": 5.6745769230769225
+      },
+      "prevControl": {
+        "x": 2.6330609248104158,
+        "y": 5.743196664476699
+      },
+      "nextControl": {
+        "x": 3.7815128205128214,
+        "y": 5.651320512820514
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.967615384615385,
+        "y": 5.709461538461539
+      },
+      "prevControl": {
+        "x": 4.746653846153847,
+        "y": 5.430384615384616
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.5211640211640187,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -91.24536426676825
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 179.59775645089275
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.935255041518388,
+        "y": 5.6650177935943065
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.073930090574693,
+        "y": 5.873030367178768
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.559893238434164,
+        "y": 7.547876631079477
+      },
+      "prevControl": {
+        "x": 7.1940806642941855,
+        "y": 7.182064056939502
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.98776039963968
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -91.19348942398209
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.45,
+        "y": 1.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.8305284220032643,
+        "y": 0.9200002390150002
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4998005698005699,
+        "y": 1.140868945868947
+      },
+      "prevControl": {
+        "x": 1.2334625881261518,
+        "y": 0.9559732945163617
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.6115541,
+        "y": 4.0213534
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.4,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.4,
+        "y": 5.5
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.2749379652605452,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -137.5
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Center-Hub-Shoot.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.35,
+        "y": 4.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 2.75,
+        "y": 4.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Center-NeutralL.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.5298452380952394,
+        "y": 4.83402380952381
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.4
+      },
+      "prevControl": {
+        "x": 2.013238095238095,
+        "y": 7.648345238095238
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Center-NeutralR.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.177702380952382,
+        "y": 3.2791666666666672
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.666
+      },
+      "prevControl": {
+        "x": 1.9210000000000012,
+        "y": 0.590559523809524
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Center-ToDepot.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.490418422348576,
+        "y": 4.249816319262193
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.115,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 2.1584120444167327,
+        "y": 5.753798061746948
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -178.85423716182484
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Copy of HubFarRight-NeutralZone 1-2.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 0.6661428571428576
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.6685357142857145,
+        "y": 0.6015000000000011
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.663,
+        "y": 0.8712976190476196
+      },
+      "prevControl": {
+        "x": 7.713544609817508,
+        "y": 0.5725975891884018
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 9.0,
+        "y": 0.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.025631067961170227,
+      "maxWaypointRelativePos": 0.6920388349514595,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.6,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (side).path

@@ -0,0 +1,107 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.115,
+        "y": 6.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9425952380952385,
+        "y": 6.874773809523809
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.52,
+        "y": 6.972
+      },
+      "prevControl": {
+        "x": 0.48937971516014256,
+        "y": 7.437592646883011
+      },
+      "nextControl": {
+        "x": 0.5506202848398575,
+        "y": 6.50640735311699
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.485,
+        "y": 4.985190476190476
+      },
+      "prevControl": {
+        "x": 0.554559516916223,
+        "y": 4.745062440329642
+      },
+      "nextControl": {
+        "x": 0.36614285714285816,
+        "y": 5.395499999999999
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.3960952380952394,
+        "y": 5.179547619047619
+      },
+      "prevControl": {
+        "x": 2.1460952380952394,
+        "y": 5.179547619047619
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.5,
+        "y": 4.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.2,
+      "maxWaypointRelativePos": 2.3115107913668993,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 4.61155415,
+        "y": 4.0213534
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.673664122137403,
+      "maxWaypointRelativePos": 3.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.4,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 500.0,
+    "maxAngularAcceleration": 700.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 147.26477372789242
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path

@@ -0,0 +1,121 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5541168091168096,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8047435897435893,
+        "y": 3.957478632478633
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.3637023809523816,
+        "y": 3.0632142857142854
+      },
+      "prevControl": {
+        "x": 2.6426055148555156,
+        "y": 3.382459299959301
+      },
+      "nextControl": {
+        "x": 1.8588436027530886,
+        "y": 2.4853303244177742
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.5754761904761914,
+        "y": 1.0
+      },
+      "prevControl": {
+        "x": 2.2272563574654534,
+        "y": 0.9727386663284083
+      },
+      "nextControl": {
+        "x": 0.9236960234869294,
+        "y": 1.0272613336715914
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.3,
+        "y": 0.8999999999999999
+      },
+      "prevControl": {
+        "x": 0.5497421350529089,
+        "y": 0.8886480847776855
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.771712158808938,
+      "maxWaypointRelativePos": 3.0,
+      "constraints": {
+        "maxVelocity": 0.7,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 1.0272952853597963,
+      "maxWaypointRelativePos": 1.95,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 4.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.44367245657569376,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -95.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.594630952380953,
+        "y": 7.425452380952382
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.690130952380953,
+        "y": 7.504892857142858
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.535761904761905,
+        "y": 7.54422619047619
+      },
+      "prevControl": {
+        "x": 7.654535714285714,
+        "y": 7.187904761904762
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 9.0,
+        "y": 7.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.025631067961170227,
+      "maxWaypointRelativePos": 0.6920388349514595,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path

@@ -0,0 +1,105 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.535761904761905,
+        "y": 7.54422619047619
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.374699934066787,
+        "y": 8.027412102561545
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.535761904761905,
+        "y": 0.666
+      },
+      "prevControl": {
+        "x": 7.809145597850186,
+        "y": 0.6686447716595759
+      },
+      "nextControl": {
+        "x": 6.434404761904762,
+        "y": 0.6553452380952387
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.666
+      },
+      "prevControl": {
+        "x": 4.246995139395343,
+        "y": 0.7046445483228254
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.2679900744416821,
+      "maxWaypointRelativePos": 0.8992555831265578,
+      "constraints": {
+        "maxVelocity": 1.2,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 6.75,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.9091262135922324,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.4,
+        "y": 0.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.9728155339805904,
+      "maxWaypointRelativePos": 1.5499999999999998,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.4
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7848095238095247,
+        "y": 7.393059523809524
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.514869047619049,
+        "y": 5.805809523809524
+      },
+      "prevControl": {
+        "x": 2.638903782208257,
+        "y": 6.0228703093406395
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.61155415,
+        "y": 4.0213534
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.40595533498758746,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 7.4
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.32258064516129836,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 139.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 1-2.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.666
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.052309523809524,
+        "y": 0.6013571428571436
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.663,
+        "y": 0.8712976190476196
+      },
+      "prevControl": {
+        "x": 7.713544609817508,
+        "y": 0.5725975891884018
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 9.0,
+        "y": 0.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.025631067961170227,
+      "maxWaypointRelativePos": 0.6920388349514595,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path

@@ -0,0 +1,105 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.663,
+        "y": 0.8712976190476196
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.578952380952382,
+        "y": 1.367988095238095
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.53,
+        "y": 7.4
+      },
+      "prevControl": {
+        "x": 7.779738811179202,
+        "y": 7.388575194058578
+      },
+      "nextControl": {
+        "x": 6.471233966716803,
+        "y": 7.448435388999068
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.4
+      },
+      "prevControl": {
+        "x": 4.246995139395343,
+        "y": 7.438644548322825
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 0.95483870967742,
+      "constraints": {
+        "maxVelocity": 1.2,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 6.75,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.9091262135922324,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.4,
+        "y": 7.4
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.9728155339805904,
+      "maxWaypointRelativePos": 1.5499999999999998,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HubFarRight-Shoot.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.666
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.17352380952381,
+        "y": 0.5905595238095237
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.61204761904762,
+        "y": 1.7351071428571436
+      },
+      "prevControl": {
+        "x": 2.8495119047619064,
+        "y": 1.4542857142857142
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.61155415,
+        "y": 4.0213534
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.40595533498758746,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.32258064516129836,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -129.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HubLeft-Shoot.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.508573466476462,
+        "y": 6.473937232524964
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.258573466476462,
+        "y": 6.473937232524964
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.029843081312411,
+        "y": 5.412967189728959
+      },
+      "prevControl": {
+        "x": 3.133352353780315,
+        "y": 5.8916975748930085
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.612,
+        "y": 4.0213534
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.18893129770992434,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 139.3987053549956
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path

@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5838333333333345,
+        "y": 1.5515476190476192
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.3365992451162603,
+        "y": 1.5886327322801814
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.5754761904761914,
+        "y": 1.0
+      },
+      "prevControl": {
+        "x": 2.2272563574654534,
+        "y": 0.9727386663284083
+      },
+      "nextControl": {
+        "x": 0.9236960234869294,
+        "y": 1.0272613336715914
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.325,
+        "y": 0.95
+      },
+      "prevControl": {
+        "x": 0.5747421350529089,
+        "y": 0.9386480847776856
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.6,
+      "maxWaypointRelativePos": 1.0956834532374118,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -95.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HubRight-Shoot.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.573035714285715,
+        "y": 1.5623452380952383
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.323035714285715,
+        "y": 1.5623452380952383
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.299,
+        "y": 2.599
+      },
+      "prevControl": {
+        "x": 2.5364642857142865,
+        "y": 2.3181785714285708
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.61155415,
+        "y": 4.0213534
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.18893129770992434,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -148.82
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LShoot-Tower.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.029843081312411,
+        "y": 5.412967189728959
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.5430833333333345,
+        "y": 5.40629761904762
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.1003809523809533,
+        "y": 4.72604761904762
+      },
+      "prevControl": {
+        "x": -0.15214285714285625,
+        "y": 4.8772142857142855
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.0,
+        "y": 5.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.10038167938931303,
+      "maxWaypointRelativePos": 0.9064885496183247,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 139.399
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/LeftBump-Neutral-Shoot.path

@@ -0,0 +1,128 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.968,
+        "y": 5.709
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.105960476887348,
+        "y": 5.9174871862178
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.781615384615385,
+        "y": 6.976935897435897
+      },
+      "prevControl": {
+        "x": 7.478867821173717,
+        "y": 7.355370351737978
+      },
+      "nextControl": {
+        "x": 7.9377891465039925,
+        "y": 6.78171869507514
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.781615384615385,
+        "y": 4.837346153846154
+      },
+      "prevControl": {
+        "x": 7.804249321126013,
+        "y": 5.08631945546308
+      },
+      "nextControl": {
+        "x": 7.758981448104757,
+        "y": 4.588372852229227
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 6.795888849032032,
+        "y": 5.623585450157039
+      },
+      "nextControl": {
+        "x": 6.348675253532072,
+        "y": 5.399978652407064
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 3.6429272202047827,
+        "y": 5.560811085066598
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": 22.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.02657807308969639,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 100.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 23.07
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4559230769230784,
+        "y": 5.709461538461539
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7466282051282063,
+        "y": 5.68620512820513
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.967615384615385,
+        "y": 5.709461538461539
+      },
+      "prevControl": {
+        "x": 4.746653846153847,
+        "y": 5.430384615384616
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5211640211640187,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -91.24536426676825
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -147.0305960965379
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LeftBump2-Neutral2-Shoot2.path

@@ -0,0 +1,148 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 5.651320512820514
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.049027667109173,
+        "y": 5.401320514351452
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.7931923076864935,
+        "y": 3.453589743589744
+      },
+      "prevControl": {
+        "x": 5.724512025720087,
+        "y": 3.693970730502218
+      },
+      "nextControl": {
+        "x": 5.90029192050797,
+        "y": 3.0787410986677135
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.316487179487182,
+        "y": 4.128025641025641
+      },
+      "prevControl": {
+        "x": 7.1826016731476,
+        "y": 3.9168984964132205
+      },
+      "nextControl": {
+        "x": 7.618820512820513,
+        "y": 4.604782051282052
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.4908846153846165,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 6.716022524475761,
+        "y": 5.620469317739846
+      },
+      "nextControl": {
+        "x": 6.153666666666667,
+        "y": 5.348987179487179
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 3.6429272202047827,
+        "y": 5.560811085066598
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": 22.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 6.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.02657807308969639,
+      "maxWaypointRelativePos": 0.9302325581395484,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 8.0,
+        "y": 5.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 1.2846068660022045,
+      "maxWaypointRelativePos": 1.7718715393134012,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 5.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 1.8870431893687745,
+      "maxWaypointRelativePos": 2.2768549280177193,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 12.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 2.5603543743078774,
+      "maxWaypointRelativePos": 3.0121816168327573,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 23.069999999999993
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.2815000000000016,
+        "y": 5.511782051282052
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.735000000000001,
+        "y": 5.593179487179486
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 5.616435897435898
+      },
+      "prevControl": {
+        "x": 4.804782051282053,
+        "y": 5.616435897435898
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.45263157894736444,
+      "rotationDegrees": 112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 91.245
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 117.25532837494306
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LeftRamp-Shoot.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.398,
+        "y": 5.512
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2417373766296596,
+        "y": 5.316853920004989
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.23498717948718,
+        "y": 5.407128205128206
+      },
+      "prevControl": {
+        "x": 3.369206052535471,
+        "y": 5.618043577061233
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.612,
+        "y": 4.021
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 133.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 113.07
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0522864367238562,
+        "y": 4.953628205128206
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.0522864367238562,
+        "y": 4.66062130060539
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0522864367238562,
+        "y": 4.662923076923078
+      },
+      "prevControl": {
+        "x": 1.0522864367238562,
+        "y": 4.912923076923078
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 4.67
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.7513725490196074,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 5.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.36705882352941094,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.1,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path

@@ -0,0 +1,152 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5373205128205143,
+        "y": 7.430435897435898
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7870116206619695,
+        "y": 7.418012103131955
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.642076923076923,
+        "y": 7.069961538461539
+      },
+      "prevControl": {
+        "x": 7.178228149431453,
+        "y": 7.5205380189223625
+      },
+      "nextControl": {
+        "x": 8.216115793872147,
+        "y": 6.51234787481454
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.781615384615385,
+        "y": 4.837346153846154
+      },
+      "prevControl": {
+        "x": 7.786585532087434,
+        "y": 5.241663699788379
+      },
+      "nextControl": {
+        "x": 7.778542446002628,
+        "y": 4.5873650404629975
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 6.957648225000392,
+        "y": 5.601152104386376
+      },
+      "nextControl": {
+        "x": 6.204516600433754,
+        "y": 5.426493771942126
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 3.7826390804558825,
+        "y": 5.585258005415613
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": 22.5
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.374307862679942,
+      "maxWaypointRelativePos": 4.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 10.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.9302325581395282,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 8.0,
+        "y": 7.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 7.4
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.6821705426356636,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 11.706999999999994
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Trench-Shoot.path

@@ -0,0 +1,173 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.594630952380953,
+        "y": 7.414654761904762
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.156107142857143,
+        "y": 7.457845238095238
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.460178571428572,
+        "y": 7.198702380952382
+      },
+      "prevControl": {
+        "x": 6.952690476190477,
+        "y": 7.522630952380953
+      },
+      "nextControl": {
+        "x": 7.89244195981076,
+        "y": 6.922789579857368
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.632940476190477,
+        "y": 4.71525
+      },
+      "prevControl": {
+        "x": 7.6161014126744035,
+        "y": 4.964682247193305
+      },
+      "nextControl": {
+        "x": 7.682415698697642,
+        "y": 3.9823876339562227
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.402011904761906,
+        "y": 6.561642857142857
+      },
+      "prevControl": {
+        "x": 6.45205334133696,
+        "y": 6.141294789912411
+      },
+      "nextControl": {
+        "x": 6.240047619047619,
+        "y": 7.922142857142857
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.594630952380953,
+        "y": 7.414654761904762
+      },
+      "prevControl": {
+        "x": 3.8777698932069846,
+        "y": 7.374206341786758
+      },
+      "nextControl": {
+        "x": 3.2690412087912097,
+        "y": 7.461167582417582
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.61204761904762,
+        "y": 6.216119047619048
+      },
+      "prevControl": {
+        "x": 3.0007619047619056,
+        "y": 6.4644642857142856
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.275434243176181,
+      "maxWaypointRelativePos": 1.8709677419354975,
+      "constraints": {
+        "maxVelocity": 1.2,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 1.3067552602436252,
+      "maxWaypointRelativePos": 2.0198511166253303,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 7.4
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.6910299003322272,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 7.4
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 2.5913621262458837,
+      "maxWaypointRelativePos": 4.28,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 4.612,
+        "y": 4.021
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 4.529346622369888,
+      "maxWaypointRelativePos": 5.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 133.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/No Move.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 2.867
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7320383800418035,
+        "y": 2.77395060350773
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 2.867
+      },
+      "prevControl": {
+        "x": 2.25,
+        "y": 2.867
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.1,
+    "maxAcceleration": 0.1,
+    "maxAngularVelocity": 0.1,
+    "maxAngularAcceleration": 0.1,
+    "nominalVoltage": 6.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -144.898
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -144.898
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.45,
+        "y": 1.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.8305284220032643,
+        "y": 0.9200002390150002
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.435,
+        "y": 1.6487261904761907
+      },
+      "prevControl": {
+        "x": 1.168662018325582,
+        "y": 1.4638305391236057
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.612,
+        "y": 4.0213534
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.4,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.4,
+        "y": 5.5
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.2749379652605452,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -142.9
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RShoot-Tower (Matt).path

@@ -0,0 +1,97 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.299,
+        "y": 2.599
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.1477500000000003,
+        "y": 2.836464285714286
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.37,
+        "y": 2.6
+      },
+      "prevControl": {
+        "x": 1.3866297526309428,
+        "y": 2.3505537105358476
+      },
+      "nextControl": {
+        "x": 1.3533702473690576,
+        "y": 2.8494462894641526
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.37,
+        "y": 2.8
+      },
+      "prevControl": {
+        "x": 1.12,
+        "y": 2.8
+      },
+      "nextControl": {
+        "x": 1.62,
+        "y": 2.8
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0522864367238562,
+        "y": 2.8
+      },
+      "prevControl": {
+        "x": 1.3005335052725924,
+        "y": 2.829553222446277
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 2.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1.7209923664121958,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.1,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 770.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -148.815
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/RShoot-Tower.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.299,
+        "y": 2.599
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.1477500000000003,
+        "y": 2.836464285714286
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0032023809523818,
+        "y": 2.7824761904761908
+      },
+      "prevControl": {
+        "x": 1.5106904761904771,
+        "y": 2.7716785714285708
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 3.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.30343511450381533,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 770.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -148.815
+  },
+  "useDefaultConstraints": false
+}

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

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.435,
+        "y": 1.649
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.607678571428572,
+        "y": 1.3897738095238092
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.666
+      },
+      "prevControl": {
+        "x": 2.871190476190477,
+        "y": 0.5689642857142869
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 1.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.5,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.5,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -148.82
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RightBump-Neutral-Shoot.path

@@ -0,0 +1,128 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.968,
+        "y": 2.7089999999999996
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.900563478191006,
+        "y": 2.468267128280525
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.723474358974359,
+        "y": 0.9767820512820515
+      },
+      "prevControl": {
+        "x": 7.037410256410256,
+        "y": 1.0930641025641024
+      },
+      "nextControl": {
+        "x": 7.969958998382976,
+        "y": 0.9350049937551674
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.863012820512822,
+        "y": 3.4419615384615385
+      },
+      "prevControl": {
+        "x": 7.88564675702345,
+        "y": 3.690934840078465
+      },
+      "nextControl": {
+        "x": 7.8403788840021935,
+        "y": 3.192988236844612
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 2.5119999999999996
+      },
+      "prevControl": {
+        "x": 6.795888849032032,
+        "y": 2.623803398874987
+      },
+      "nextControl": {
+        "x": 6.348675253532072,
+        "y": 2.400196601125012
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 2.512
+      },
+      "prevControl": {
+        "x": 3.6429272202047827,
+        "y": 2.5610290337845467
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": -22.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.02657807308969639,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": -100.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -23.069999999999993
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 88.32335186314447
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4559230769230784,
+        "y": 2.5465897435897444
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7466282051282063,
+        "y": 2.523333333333337
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.967615384615385,
+        "y": 2.7089999999999996
+      },
+      "prevControl": {
+        "x": 4.746653846153847,
+        "y": 2.4299230769230773
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5211640211640187,
+      "rotationDegrees": 112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 91.245
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 127.62823656328939
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RightBump2-Neutral2-Shoot2.path

@@ -0,0 +1,142 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 2.5119999999999996
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.049027667109173,
+        "y": 2.2620000015309376
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 4.8140897435897445
+      },
+      "prevControl": {
+        "x": 5.8000264938666355,
+        "y": 4.791458056872672
+      },
+      "nextControl": {
+        "x": 6.297973506133364,
+        "y": 4.836721430306816
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.374628205128206,
+        "y": 4.267564102564102
+      },
+      "prevControl": {
+        "x": 7.209222795856494,
+        "y": 4.455023566405375
+      },
+      "nextControl": {
+        "x": 7.549051282051284,
+        "y": 4.069884615384615
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.7118205128205135,
+        "y": 2.779153846153846
+      },
+      "prevControl": {
+        "x": 6.936958421911658,
+        "y": 2.8878411126116403
+      },
+      "nextControl": {
+        "x": 6.374602564102564,
+        "y": 2.6163589743589735
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 2.5119999999999996
+      },
+      "prevControl": {
+        "x": 3.642927220204783,
+        "y": 2.5610290337845445
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.18947368421052632,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": -22.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 6.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.23034330011073328,
+      "maxWaypointRelativePos": 0.868217054263568,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 8.0,
+        "y": 2.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 1.2846068660022045,
+      "maxWaypointRelativePos": 1.7718715393134012,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 5.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 2.1085271317829366,
+      "maxWaypointRelativePos": 3.1539313399778472,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -23.069999999999993
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5256923076923092,
+        "y": 2.6159999999999997
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8279615384615413,
+        "y": 1.9764871794871786
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 2.6159999999999997
+      },
+      "prevControl": {
+        "x": 5.002474358974361,
+        "y": 2.488448717948718
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5211640211640187,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -91.245
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -108.98741511036879
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RightRamp-Shoot.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.398,
+        "y": 2.5119999999999996
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.160642585150445,
+        "y": 2.590494952805491
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.398,
+        "y": 2.5119999999999996
+      },
+      "prevControl": {
+        "x": 3.595338054344081,
+        "y": 2.3585148466212686
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.612,
+        "y": 4.021
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 0.1,
+    "maxAngularAcceleration": 0.1,
+    "nominalVoltage": 6.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -129.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -23.07
+  },
+  "useDefaultConstraints": false
+}

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

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0522864367238562,
+        "y": 2.4768205128205136
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.0771623664791057,
+        "y": 2.725579810373011
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0522864367238562,
+        "y": 2.8
+      },
+      "prevControl": {
+        "x": 1.0473432640619955,
+        "y": 2.5500488746894003
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 2.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.3999999999999999,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.1,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/RightTrench-FirstSwipe.path

@@ -0,0 +1,131 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.8969642857142865,
+        "y": 0.6757407407407425
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.1494880952380955,
+        "y": 0.6013571428571436
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.237607142857144,
+        "y": 1.4759642857142854
+      },
+      "prevControl": {
+        "x": 7.558649163266191,
+        "y": 0.2980245529994656
+      },
+      "nextControl": {
+        "x": 8.584042208300819,
+        "y": 2.0770024068622486
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.237607142857144,
+        "y": 3.8190476190476197
+      },
+      "prevControl": {
+        "x": 8.310074297364315,
+        "y": 3.5787466468913385
+      },
+      "nextControl": {
+        "x": 8.165425863946698,
+        "y": 4.058400628167732
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.237607142857144,
+        "y": 1.0872500000000003
+      },
+      "prevControl": {
+        "x": 9.520930827264253,
+        "y": 2.0312275985936155
+      },
+      "nextControl": {
+        "x": 7.304441714310592,
+        "y": 0.40083918172158384
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.4434642857142865,
+        "y": 0.6757407407407425
+      },
+      "prevControl": {
+        "x": 4.9917133086412,
+        "y": 0.5083004760390194
+      },
+      "nextControl": {
+        "x": 2.6336428571428576,
+        "y": 0.7633214285714289
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.61204761904762,
+        "y": 1.918666666666668
+      },
+      "prevControl": {
+        "x": 2.502511133859423,
+        "y": 1.6751404175218187
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "end of RIght trench first swipe"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.9709543568464838,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 3.4315352697095514,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 3.991228070175485,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -133.33166255170664
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path

@@ -0,0 +1,152 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6070897435897447,
+        "y": 0.6046794871794885
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8833980386504074,
+        "y": 0.5273348158056345
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.595564102564104,
+        "y": 0.9884102564102579
+      },
+      "prevControl": {
+        "x": 7.151835398129718,
+        "y": 0.5429530943613806
+      },
+      "nextControl": {
+        "x": 7.93843114627953,
+        "y": 1.332612871042205
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.793243589743589,
+        "y": 3.4070769230769233
+      },
+      "prevControl": {
+        "x": 7.74538518514461,
+        "y": 3.9526634447090423
+      },
+      "nextControl": {
+        "x": 7.8344917146104915,
+        "y": 2.936847688346859
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 2.5119999999999996
+      },
+      "prevControl": {
+        "x": 7.050706524183416,
+        "y": 2.443033938208306
+      },
+      "nextControl": {
+        "x": 6.24953920807272,
+        "y": 2.5585241728388555
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 2.512
+      },
+      "prevControl": {
+        "x": 3.7815128205128214,
+        "y": 2.5582179487179486
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": -22.5
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.2059800664451794,
+      "maxWaypointRelativePos": 4.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 10.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.859357696566985,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 5.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.6
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.6910299003322272,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -23.07
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/RightTrench-Neutral-Trench-Shoot.path

@@ -0,0 +1,173 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6070897435897447,
+        "y": 0.6046794871794885
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.115752584653353,
+        "y": 0.5635297641960387
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.470976190476191,
+        "y": 0.9884102564102579
+      },
+      "prevControl": {
+        "x": 7.2683607377306005,
+        "y": 0.7987129676949314
+      },
+      "nextControl": {
+        "x": 7.845328732990856,
+        "y": 1.3388951842695742
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.595564102564104,
+        "y": 3.3979404761904766
+      },
+      "prevControl": {
+        "x": 7.578725039048031,
+        "y": 3.6473727233837807
+      },
+      "nextControl": {
+        "x": 7.645039325071269,
+        "y": 2.665078110146699
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.3306071428571435,
+        "y": 0.9884102564102579
+      },
+      "prevControl": {
+        "x": 7.602572796768329,
+        "y": 1.2858726903756175
+      },
+      "nextControl": {
+        "x": 6.958504578754579,
+        "y": 0.5814230769230777
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.711743589743591,
+        "y": 0.6046794871794885
+      },
+      "prevControl": {
+        "x": 3.9948825305696225,
+        "y": 0.5642310670614844
+      },
+      "nextControl": {
+        "x": 3.3861538461538476,
+        "y": 0.6511923076923085
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.4442692307692324,
+        "y": 1.302
+      },
+      "prevControl": {
+        "x": 3.3396410256410265,
+        "y": 1.1395769230769235
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "END-RightTrnech-Neutral-Trench-Shoot"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.255583126550872,
+      "maxWaypointRelativePos": 1.7617866004962806,
+      "constraints": {
+        "maxVelocity": 1.2,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 1.3067552602436252,
+      "maxWaypointRelativePos": 2.015503875969026,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.6
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.6910299003322272,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.5913621262458837,
+      "maxWaypointRelativePos": 4.28,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 4.612,
+        "y": 4.021
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 4.529346622369888,
+      "maxWaypointRelativePos": 5.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -130.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Shoot driving across.path

@@ -0,0 +1,102 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 6.5815769230769225
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9932621113872893,
+        "y": 6.3390412980405895
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 1.6628461538461545
+      },
+      "prevControl": {
+        "x": 1.936134248900721,
+        "y": 1.9128215679513527
+      },
+      "nextControl": {
+        "x": 1.9291221613556924,
+        "y": 1.4128707397409563
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 6.5815769230769225
+      },
+      "prevControl": {
+        "x": 1.9198835421653975,
+        "y": 6.331901987278464
+      },
+      "nextControl": {
+        "x": 1.9453728680910158,
+        "y": 6.8312518588753814
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 1.6628461538461545
+      },
+      "prevControl": {
+        "x": 1.9337461053582308,
+        "y": 1.912843654431812
+      },
+      "nextControl": {
+        "x": 1.9315103048981825,
+        "y": 1.4128486532604971
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 6.5815769230769225
+      },
+      "prevControl": {
+        "x": 1.8878500514131944,
+        "y": 6.335619778537727
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.8,
+    "maxAcceleration": 10.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/ShotL-Neutral.path

@@ -0,0 +1,215 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.612,
+        "y": 6.216
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.958273419839204,
+        "y": 6.2585504440271835
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.595,
+        "y": 7.415
+      },
+      "prevControl": {
+        "x": 2.756519548465237,
+        "y": 7.377436791520136
+      },
+      "nextControl": {
+        "x": 4.203016743276786,
+        "y": 7.4422386310797695
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.786547619047619,
+        "y": 7.285083333333334
+      },
+      "prevControl": {
+        "x": 5.572245332631391,
+        "y": 7.447981654135905
+      },
+      "nextControl": {
+        "x": 6.56590740520863,
+        "y": 6.692665946707327
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.786547619047619,
+        "y": 3.959416666666667
+      },
+      "prevControl": {
+        "x": 5.556087373096201,
+        "y": 4.228987726737335
+      },
+      "nextControl": {
+        "x": 6.036389134520053,
+        "y": 3.6671751917974453
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.58557142857143,
+        "y": 4.27254761904762
+      },
+      "prevControl": {
+        "x": 6.494472316478745,
+        "y": 3.9148971897508758
+      },
+      "nextControl": {
+        "x": 6.681350061848797,
+        "y": 4.648569607870673
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.315630952380953,
+        "y": 7.285083333333334
+      },
+      "prevControl": {
+        "x": 6.842509529997394,
+        "y": 7.164317956805786
+      },
+      "nextControl": {
+        "x": 5.287977497592714,
+        "y": 7.520630880111148
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.594630952380953,
+        "y": 7.414654761904762
+      },
+      "prevControl": {
+        "x": 3.844619709717091,
+        "y": 7.417025670071149
+      },
+      "nextControl": {
+        "x": 3.0970699426307586,
+        "y": 7.409935863847653
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.612,
+        "y": 6.216
+      },
+      "prevControl": {
+        "x": 2.844455287324918,
+        "y": 6.424977756526983
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.650124069478908,
+      "maxWaypointRelativePos": 2.8709677419354973,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 6.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 2.306755260243625,
+      "maxWaypointRelativePos": 3.3975186104218427,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 7.4
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1.4,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 7.4
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 4.689826302729521,
+      "maxWaypointRelativePos": 6.28,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 4.612,
+        "y": 4.021
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 6.529346622369888,
+      "maxWaypointRelativePos": 7.0,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 6.0,
+        "y": 0.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 3.7,
+      "maxWaypointRelativePos": 4.5786600496277865,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 133.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 133.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/ShotRightR-PlayerStation.path

@@ -0,0 +1,95 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.398,
+        "y": 2.512
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2508028935074877,
+        "y": 2.309928201274319
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.5754761904761914,
+        "y": 1.0
+      },
+      "prevControl": {
+        "x": 2.2272563574654534,
+        "y": 0.9727386663284083
+      },
+      "nextControl": {
+        "x": 0.9236960234869294,
+        "y": 1.0272613336715914
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.325,
+        "y": 0.95
+      },
+      "prevControl": {
+        "x": 0.5747421350529089,
+        "y": 0.9386480847776856
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 10.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.6,
+      "maxWaypointRelativePos": 1.0956834532374118,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -95.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -106.71386741074437
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/ShotRightT-PlayerStation.path

@@ -0,0 +1,95 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.4442692307692324,
+        "y": 1.302
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.29707212427672,
+        "y": 1.0999282012743195
+      },
+      "isLocked": false,
+      "linkedName": "END-RightTrnech-Neutral-Trench-Shoot"
+    },
+    {
+      "anchor": {
+        "x": 1.5754761904761914,
+        "y": 1.0
+      },
+      "prevControl": {
+        "x": 2.2272563574654534,
+        "y": 0.9727386663284083
+      },
+      "nextControl": {
+        "x": 0.9236960234869294,
+        "y": 1.0272613336715914
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.325,
+        "y": 0.95
+      },
+      "prevControl": {
+        "x": 0.5747421350529089,
+        "y": 0.9386480847776856
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Playerstation"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 0.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 10.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.6,
+      "maxWaypointRelativePos": 1.0956834532374118,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -95.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -130.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,163 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6070897435897447,
+        "y": 0.6046794871794885
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.115752584653353,
+        "y": 0.5635297641960387
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.470976190476191,
+        "y": 0.9884102564102579
+      },
+      "prevControl": {
+        "x": 7.2683607377306005,
+        "y": 0.7987129676949314
+      },
+      "nextControl": {
+        "x": 7.845328732990856,
+        "y": 1.3388951842695742
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.595564102564104,
+        "y": 3.3979404761904766
+      },
+      "prevControl": {
+        "x": 7.578725039048031,
+        "y": 3.6473727233837807
+      },
+      "nextControl": {
+        "x": 7.645039325071269,
+        "y": 2.665078110146699
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.3306071428571435,
+        "y": 0.9884102564102579
+      },
+      "prevControl": {
+        "x": 7.602572796768329,
+        "y": 1.2858726903756175
+      },
+      "nextControl": {
+        "x": 6.958504578754579,
+        "y": 0.5814230769230777
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.712,
+        "y": 0.6046794871794885
+      },
+      "prevControl": {
+        "x": 3.997989714851531,
+        "y": 0.6083696770485412
+      },
+      "nextControl": {
+        "x": 3.4545183150183143,
+        "y": 0.6013571428571436
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.472,
+        "y": 0.6046794871794885
+      },
+      "prevControl": {
+        "x": 2.721109806922687,
+        "y": 0.6257580093037158
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.255583126550872,
+      "maxWaypointRelativePos": 1.7617866004962806,
+      "constraints": {
+        "maxVelocity": 1.2,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 1.3067552602436252,
+      "maxWaypointRelativePos": 2.015503875969026,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.6
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.6910299003322272,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.5913621262458837,
+      "maxWaypointRelativePos": 5.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -0,0 +1,95 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.472,
+        "y": 0.605
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2708330107691865,
+        "y": 0.7534312717853275
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.5754761904761914,
+        "y": 1.0
+      },
+      "prevControl": {
+        "x": 2.2272563574654534,
+        "y": 0.9727386663284083
+      },
+      "nextControl": {
+        "x": 0.9236960234869294,
+        "y": 1.0272613336715914
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.325,
+        "y": 0.95
+      },
+      "prevControl": {
+        "x": 0.5747421350529089,
+        "y": 0.9386480847776856
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 0.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 10.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.6,
+      "maxWaypointRelativePos": 1.0956834532374118,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -95.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.5365714285714285
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.119535714285715,
+        "y": 0.7633214285714292
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.547261904761905,
+        "y": 3.160392857142857
+      },
+      "prevControl": {
+        "x": 2.320511904761905,
+        "y": 2.696095238095238
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -159.67686317033716
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 177.94339863785623
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

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

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

@@ -1,188 +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 edu.wpi.first.math.geometry.Translation2d;
-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.36962890625  + 0.5;
-          public static final double FRONT_RIGHT_ROT_OFFSET =  0.61474609375  + 0.5;
-          public static final double BACK_LEFT_ROT_OFFSET   = -0.227294921875 + 0.5;
-          public static final double BACK_RIGHT_ROT_OFFSET  =  0.60595703125  + 0.5;
-        }
-
-        public static final class IDs {
-            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 = 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 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 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 double APRIL_HEIGHT = -1.0; // TODO: find actual value
-
-        public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609);
-        public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593);
-
-        public static final double SpeakerBubbleDistance = 3;
-        public static final double targetPosDistance = 1.5;
-    
-    }
-
-    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 ShooterConstants {
-        public static final int LEFT_SHOOTER_ID = 15; // final
-        public static final int RIGHT_SHOOTER_ID = 16; // final
-        public static final double SHOOTER_SPEED = 0.50; // final
-        public static final double SHOOTER_IDLE = 0.20; // final
-        public static final double SHOOTER_IDLE_LIMELIGHT = 0.20;
-    }
-
-    public static final class IntakeConstants {
-        public static final int INTAKE_MOTOR_ID = 18;
-        public static final int PIVOT_MOTOR_ID = 17;
-        public static final double INTAKE_SPEED = 0.75;
-        public static final double INTAKE_OUT_SPEED_UNPRESSED =  1.0;       
-        public static final double INTAKE_OUT_SPEED_PRESSED = 0.5;        
-        public static final double HANDOFF_SPEED = 0.4;
-        public static final double PIVOT_SPEED = 0.2;
-
-        public static final class ArmPID {
-            public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0);
-        }
-    }
-
-    public static final class ClimbConstants {
-        public static final int CLIMB_MOTOR_ID = 19;
-        public static final double CLIMB_IN_SPEED = -1.0;
-        public static final double CLIMB_OUT_SPEED = 0.87;
-    }
-
-    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.1;
-
-    }
-}

src/main/java/frc4388/robot/Main.java

@@ -25,5 +25,6 @@ public final class Main {
    */
   public static void main(String... args) {
     RobotBase.startRobot(Robot::new);
+    
   }
 }

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

@@ -7,11 +7,28 @@
 
 package frc4388.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+// 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 com.ctre.phoenix6.SignalLogger;
+
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 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.HubShiftTimer;
+import frc4388.utility.compute.HubShiftTimer.ShiftInfo;
+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
@@ -20,7 +37,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();
@@ -32,12 +49,23 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotInit() {
+    // Start logging with AdvantageKit
+    startLogging();
+
+    com.revrobotics.util.StatusLogger.disableAutoLogging();
+    SignalLogger.enableAutoLogging(false);
 
     // 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,
@@ -49,15 +77,13 @@ public class Robot extends TimedRobot {
   @Override
   public void robotPeriodic() {   
     m_robotTime.updateTimes();
-    //System.out.println(m_robotContainer.limelight.isNearSpeaker());
-    //mled.updateLED();
     // 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
@@ -96,6 +122,7 @@ public class Robot extends TimedRobot {
       m_autonomousCommand.schedule();
     }
     m_robotTime.startMatchTime();
+    HubShiftTimer.initializeAuto();
   }
 
   /**
@@ -105,16 +132,23 @@ 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();
+    HubShiftTimer.initializeTeleop();
   }
 
   /**
@@ -122,13 +156,121 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-  //  m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
+    var info = HubShiftTimer.getShiftInfo();
+
+    double rumble = (info.remainingInShift() < 5.  && info.remainingInShift() > 0.1) ? 1 : 0;
+
+    // m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble);
+    // m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble);
   }
 
   /**
-   * 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

@@ -5,303 +5,306 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+// 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;
 
-// Drive Systems
+import java.io.File;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.wpilibj.GenericHID;
-import frc4388.utility.controller.XboxController;
-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;
 
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
+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.wpilibj.DriverStation;
+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;
 // Commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
+import edu.wpi.first.wpilibj2.command.button.Trigger;
-// Autos
+import frc4388.robot.commands.Swerve.StayInPosition;
-import frc4388.robot.commands.Intake.ArmIntakeIn;
+import frc4388.robot.constants.Constants;
-import frc4388.utility.controller.VirtualController;
+import frc4388.robot.constants.Constants.OIConstants;
-import frc4388.robot.commands.Swerve.neoJoystickPlayback;
+import frc4388.robot.constants.Constants.SimConstants.Mode;
-import frc4388.robot.commands.Swerve.neoJoystickRecorder;
+import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
-
+import frc4388.robot.subsystems.swerve.SwerveDrive;
-// Subsystems
+import frc4388.robot.subsystems.vision.Vision;
-import frc4388.robot.subsystems.Climber;
-import frc4388.robot.subsystems.Intake;
-import frc4388.robot.subsystems.Limelight;
-import frc4388.robot.subsystems.Shooter;
-// import frc4388.robot.subsystems.LED;
-import frc4388.robot.subsystems.SwerveDrive;
-
-// Utilites
 import frc4388.utility.DeferredBlock;
-import frc4388.utility.configurable.ConfigurableString;
+import frc4388.utility.compute.FieldPositions;
+import frc4388.utility.compute.TimesNegativeOne;
+import frc4388.utility.controller.DeadbandedXboxController;
+// Autos
+import frc4388.utility.controller.VirtualController;
+import frc4388.utility.controller.XboxController;
 
 /**
  * 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 = new RobotMap(RobotBase.isReal() ? Mode.REAL : Mode.SIM);
+    
+    /*Limit Switch */
+    // public final DigitalInput m_armLimitSwitch = new DigitalInput(9);
+
     /* Subsystems */
-    // private final LED m_robotLED = new LED();
+    // public final Lidar m_lidar = new Lidar();
-
+    // public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
-    private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor);
+    public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim();
-
+    //Testing of Colors
-    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
+    public final Vision m_vision = new Vision();
-                                                                  m_robotMap.rightFront,
+    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
-                                                                  m_robotMap.leftBack,
+    // public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO, m_robotSwerveDrive);
-                                                                  m_robotMap.rightBack,
+    // public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
-                                              
+    
-                                                                  m_robotMap.gyro);
 
     /* 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_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);
+
+    // public List<Subsystem> subsystems = new ArrayList<>();
+    private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive);
     
-    private final Limelight limelight = new Limelight();
+    private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d());
-
+        // ! Teleop Commands
-    private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight);
+        public void stop() {
-
+            new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
-    private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor);
+            m_robotSwerveDrive.stopModules();
-
+            Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
-    /* Virtual Controllers */
+        }
-    private final VirtualController m_virtualDriver = new VirtualController(0);
-    private final VirtualController m_virtualOperator = new VirtualController(1);
-
-    private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
-    private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
-
-    // ! Teleop Commands
-
-    private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotIntake.PIDIn()),
-        new InstantCommand(() -> m_robotShooter.idle())
-    );
     
-    private SequentialCommandGroup intakeNotePullInIdle = new SequentialCommandGroup(
+        // ! /*  Autos */
-        intakeToShootStuff, intakeToShoot, 
+        private SendableChooser<String> autoChooser;
-        new InstantCommand(() -> m_robotShooter.idle())
+        private Command autoCommand;
-    );
+     
 
-    private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
+  
-        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)
-        // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
-    );
-
-    private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
-        interrupt,
-        new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake),
-        new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)
-    );
-
-    // ! /*  Autos */
-    private String lastAutoName = "four_note_taxi_kracken.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 neoJoystickPlayback ampShoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto",
+        
-        new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
+        public RobotContainer() {
-        false, true);
-
-    /**
-     * The container for the robot. Contains subsystems, OI devices, and commands.
-     */
-    public RobotContainer() {
-        configureButtonBindings();        
-        configureVirtualButtonBindings();
-        new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip());
-        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
-        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-            m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
-                                            getDeadbandedDriverController().getRight(),
-                                true);
-        }, m_robotSwerveDrive)
-        .withName("SwerveDrive DefaultCommand"));
-        m_robotSwerveDrive.setToSlow();
-
-        // ! Swerve Drive One Module Test
-        // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-        //     m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft());
-        // }
-
-        // ! Swerve Drive Default Command (Orientation Rotation)
-        // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-        //     m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(), 
-        //                                                  getDeadbandedDriverController().getRightX(), 
-        //                                                  getDeadbandedDriverController().getRightY(), 
-        //                                                  true);
-        // }, m_robotSwerveDrive))
-        // .withName("SwerveDrive OrientationCommand"));
-        // continually sends updates to the Blinkin LED controller to keep the lights on
-        // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
-
-        // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-        //     m_robotSwerveDrive.driveWithInput(
-        //                                     getDeadbandedDriverController().getLeft(), 
-        //                                     getDeadbandedDriverController().getRight(),
-        //                                     true);
-        // }, m_robotSwerveDrive));
-
-
-
-
-    }
-
-    /**
-     * Use this method to define your button->command mappings. Buttons can be
-     * created by instantiating a {@link GenericHID} or one of its subclasses
-     * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
-     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-     */
-    private void configureButtonBindings() {
-
-        // ? /* Driver Buttons */
-
-        DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip()));
             
-        // ! /* Speed */
+            configureSINGLEBindings();
+            
+            // Called on first robot enable
+            DeferredBlock.addBlock(() -> {
+                m_robotSwerveDrive.resetGyro();
+            }, false);
+    
+            // Called on every robot enable
+            DeferredBlock.addBlock(() -> {
+                // m_robotIntake.setMode(IntakeMode.Idle);
+                // m_robotShooter.spinUpIdle();
+                // m_robotIntake.io.updateGains();
+                TimesNegativeOne.update();
+                FieldPositions.update();
+                // m_robotShooter.io.updateGains();
+            }, true);
+
+            DriverStation.silenceJoystickConnectionWarning(true);
+    
+            // Drive normally
+            m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+                m_robotSwerveDrive.driveWithInput(
+                    getDeadbandedDriverController().getLeft(),
+                    getDeadbandedDriverController().getRight(),true);
+    
+            }, m_robotSwerveDrive)
+            .withName("SwerveDrive DefaultCommand"));
+            
+            m_robotSwerveDrive.setToSlow();
+            
+            makeAutoChooser();
+            SmartDashboard.putData("Auto Chooser", autoChooser);
+    
+        }
+        
+    
+
+   private boolean lt_down() {
+        return getDeadbandedDriverController().getLeftTriggerAxis() > 0.8;
+   }
+
+   private boolean rt_down() {
+        return getDeadbandedDriverController().getRightTriggerAxis() > 0.8;
+   }
+
+   private void configureSINGLEBindings() {
+
+        String controllerInstructions = "" +
+            "Single Controller:\n" + 
+
+            // Driver controls.
+            "- Sticks: Field oriented controls\n" +
+            "- Right Bumper: Shift Up\n" +
+            "- Left Bumper: Shift Down\n"  +
+            "- BACK (left small btn): Reset Gyro\n" +
+            "- DPAD: Fine Alignment\n" +
+
+            // Operator normal buttons
+            "- X (press): Roller Down\n" +
+            "- X (hold): Roller Spin\n" + 
+            "- B (hold): Roller Expel + Arm Stop\n" + 
+            "- Y : Roller Off + Arm Up \n" +
+            
+            // Operator override buttons
+            "- LT (hold): Switch over to OP override mode\n" +
+            "- LS Left+Right Override: Manually move intake\n" +
+            "- RS Up+Down Override: Manually move climber";
+
+        SmartDashboard.putString("Controller Binds", controllerInstructions);
+
+        // Driver controls
+        new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
+
         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)
+        // Fine Alignment
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
+        new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
-
+            .whileTrue(new RunCommand(
-        new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
+                () -> m_robotSwerveDrive.driveFine(
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
+                    new Translation2d(
-          
+                        1, 
-        // ?  /* Operator Buttons */
+                        Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
-            
+                    ), 
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
+                    getDeadbandedDriverController().getRight(), 0.15
-        .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn()))
+                ), m_robotSwerveDrive))
-            .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
-            
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut()))
-            .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
-
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotIntake.handoff()))
-            .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors()));
-            
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
-             .onTrue(emergencyRetract);
-
-
-        // Override Intake Position encoder: out
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake));
-            
-        // Override Intake Position encoder: in
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake));
-
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter))
-            .onFalse(turnOffShoot);
-            
-
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
-            .onTrue(intakeNotePullInIdle)
-            .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn()));
-            
-        // Spins up shooter, no wind down
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
-
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
-            .onTrue(emergencyRetract);
-
-        new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
-            .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
-            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
-
-        new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
-            .onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
-            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
-
-        // new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
-        //     .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5)));
         
-        new Trigger(() -> getDeadbandedOperatorController().getPOV() != -1)
-            .onTrue(ampShoot)
-            .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter, m_robotSwerveDrive));
         
-        // ? /* Programer Buttons (Controller 3)*/
 
-        // * /* Auto Recording */
-        new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
-            .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
-                        new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
-                                            () -> autoplaybackName.get()))
-            .onFalse(new InstantCommand());
         
-        new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
+        // Operator controls (Non-override, LT UP)
-            .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
-            () -> autoplaybackName.get(),
-            new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
-            true, false))
-            .onFalse(new InstantCommand());
-    }
-    
-    /**
-     * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}. <p/>
-     * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons.
-     */
-    private void configureVirtualButtonBindings() {
 
-        // ? /* Driver Buttons */
+
+        // Arm down
+        new Trigger(() -> !lt_down() && getDeadbandedDriverController().getXButton())
+            .onTrue(new InstantCommand(() -> {
+                m_robotMap.m_robotIntake.PIDIn();
+                m_robotMap.m_robotIntake.spinIntakeMotor();
+            }, m_robotMap.m_robotIntake))
+            .onFalse(new InstantCommand(() -> m_robotMap.m_robotIntake.stopArmMotor(), m_robotMap.m_robotIntake));
         
-        /* Notice: the following buttons have not been replicated
+        // Arm up
-         * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback
+        new Trigger(() -> !lt_down() && getDeadbandedDriverController().getYButton())
-         * Swerve Drive Rotation Gear Shifts           : Same reason as Slow and Fast mode.
+            .onTrue(new InstantCommand(() -> {
-         * Auto Recording controls                     : We don't want an Null Ouroboros for an auto.
+                m_robotMap.m_robotIntake.PIDIn();
-         */
+                m_robotMap.m_robotIntake.stopIntakeMotors();
-
+            }, m_robotMap.m_robotIntake))
-        // ? /* Operator Buttons */
+            .onFalse(new InstantCommand(() -> m_robotMap.m_robotIntake.stopArmMotor(), m_robotMap.m_robotIntake));
-
+        
-        /* Notice: the following buttons have not been replicated
+        // Handoff / spit out
-         * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot.
+        new Trigger(() -> !lt_down() && getDeadbandedDriverController().getAButton())
-         *                                    We don't need it in an auto.
+            .onTrue(new InstantCommand(() -> {
-         * Climbing controls                : We don't need to climb in auto.
+                m_robotMap.m_robotIntake.PIDIn();
-         */
+                m_robotMap.m_robotIntake.stopIntakeMotors();
+            }, m_robotMap.m_robotIntake))
+            .onFalse(new InstantCommand(() -> m_robotMap.m_robotIntake.stopArmMotor(), m_robotMap.m_robotIntake));
+
+        // Shoot
+        new Trigger(() -> !lt_down() && rt_down())
+            .onTrue(new InstantCommand(() -> m_robotMap.m_robotShooter.spin(0.5), m_robotMap.m_robotShooter))
+            .onFalse(new InstantCommand(() -> m_robotMap.m_robotShooter.stop(), m_robotMap.m_robotShooter));
+            
+
+
+        // OP Override
         
-         // ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto.
 
     }
 
+//.onTrue(new InstantCommand(()  -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));
+
     /**
      * Use this to pass the autonomous command to the main {@link Robot} class.
      *
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        //no auto
+
-        return autoPlayback;
+
-        //return playbackChooser.getCommand();
+        //return autoPlayback;
-        // return new Command() {};
+        //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>();
+        autoChooser.setDefaultOption("None", "None");
+        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("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2026KPopRobotHunters\\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 == null || filename.equals("None")) {
+                autoCommand = null;
+                return;
+            }
+            // 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);
+
+            //----
+            PathPlannerAuto auto = new PathPlannerAuto(filename);
+            m_robotSwerveDrive.setInitalPose(auto.getStartingPose());
+            //-----
+        });
+        SmartDashboard.putData(autoChooser);
+
     }
 
     /**
@@ -322,11 +325,7 @@ public class RobotContainer {
         return this.m_operatorXbox;
     }
 
-    public VirtualController getVirtualDriverController() {
+    // public ButtonBox getButtonBox() {
-        return m_virtualDriver;
+    //     return this.m_buttonBox;
-    }
+    // }
-
+}
-    public VirtualController getVirtualOperatorController() {
-        return m_virtualOperator;
-    }
-}

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

@@ -8,79 +8,219 @@
 package frc4388.robot;
 
 import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.Pigeon2;
 
-// import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import org.photonvision.PhotonCamera;
-// import frc4388.robot.Constants.LEDConstants;
+
-import frc4388.robot.Constants.SwerveDriveConstants;
+import com.ctre.phoenix6.hardware.CANcoder;
-import frc4388.robot.Constants.ShooterConstants;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
-import frc4388.robot.Constants.ClimbConstants;
+import com.revrobotics.spark.SparkMax;
-import frc4388.robot.Constants.IntakeConstants;
+
-import frc4388.robot.subsystems.SwerveModule;
+import edu.wpi.first.wpilibj.DigitalInput;
-import frc4388.utility.RobotGyro;
+import frc4388.robot.constants.ClimbConstants;
+import frc4388.robot.constants.Constants;
+//import frc4388.robot.constants.Constants.ElevatorConstants;
+import frc4388.robot.constants.Constants.SimConstants;
+import frc4388.robot.constants.IntakeConstants;
+import frc4388.robot.constants.ShooterConstants;
+import frc4388.robot.subsystems.Climber;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Shooter;
+// import frc4388.robot.constants.Constants.VisionConstants;
+// import frc4388.robot.subsystems.intake.IntakeConstants;
+// import frc4388.robot.subsystems.intake.IntakeIO;
+// import frc4388.robot.subsystems.intake.IntakeReal;
+// import frc4388.robot.subsystems.shooter.ShooterConstants;
+// import frc4388.robot.subsystems.shooter.ShooterIO;
+// import frc4388.robot.subsystems.shooter.ShooterReal;
+import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
+// import frc4388.robot.subsystems.elevator.ElevatorIO;
+// import frc4388.robot.subsystems.elevator.ElevatorReal;
+import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
+import frc4388.robot.subsystems.swerve.SwerveIO;
+import frc4388.robot.subsystems.swerve.SwerveReal;
+import frc4388.robot.subsystems.vision.VisionIO;
+import frc4388.robot.subsystems.vision.VisionReal;
+import frc4388.utility.compute.JankCoder;
+import frc4388.utility.status.FaultCANCoder;
+import frc4388.utility.status.FaultPhotonCamera;
+import frc4388.utility.status.FaultPidgeon2;
+import frc4388.utility.status.FaultSparkMax;
+import frc4388.utility.status.FaultTalonFX;
 
 /**
  * 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);
+    // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
-    public RobotGyro gyro = new RobotGyro(m_pigeon2);
+    // public RobotGyro gyro = new RobotGyro(m_pigeon2);
-    
+
-    public SwerveModule leftFront;
+    // public final VisionIO leftCamera;
-    public SwerveModule rightFront;
+    // public final VisionIO rightCamera;
-    public SwerveModule leftBack;
+
-    public SwerveModule rightBack;
+    // public final LiDAR lidar = new 
+
+    // public final LidarIO reefLidar;
+    // public final LidarIO reverseLidar;
 
-    public RobotMap() {
-        configureLEDMotorControllers();
-        configureDriveMotorControllers();
-    }
 
     /* 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 SwerveIO swerveDrivetrain;
-    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 Intake m_robotIntake;
-    public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
+    public final Shooter m_robotShooter;
-    public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
+    public final Climber m_robotClimber;
-    
-    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);
 
-    /* Shooter Subsystem */
-    public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID);
-    public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID);
 
-    /* Intake Subsystem */
+    public RobotMap(SimConstants.Mode mode) {
-    public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID);
+        
-    public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID);
+        /* Shooter Subsystem */
+        final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID.id);
+        final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID.id);
 
-    /* Climber Subsystem */
+        /* Intake Subsystem */
-    public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID);
+        final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID.id);
+        final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID.id);
 
-    void configureLEDMotorControllers() {    
+        /* Climber Subsystem */
+        final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID.id);
+
+        m_robotIntake = new Intake(intakeMotor, pivotMotor);
+        m_robotShooter = new Shooter(leftShooter, rightShooter);
+        m_robotClimber = new Climber(climbMotor);
+
+        FaultTalonFX.addDevice(leftShooter, "Left Shooter");
+        FaultTalonFX.addDevice(rightShooter, "Right Shooter");
+        FaultTalonFX.addDevice(intakeMotor, "Intake Motor");
+        FaultTalonFX.addDevice(pivotMotor, "Pivot Motor");
+        FaultTalonFX.addDevice(climbMotor, "Climb Motor");
+
+
+        switch (mode) {
+            case REAL:
+                // // Configure cameras
+                // PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
+                // PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
+
+                // leftCamera =  new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS);                ;
+                // rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
+
+                // FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
+                // FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
+
+                // // Configure LiDAR
+                // reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
+                // reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
+                // DigitalInput armLimitSwitch = new DigitalInput(IntakeConstants.ARM_LIMIT_SWITCH_CHANNEL);
+
+                // Configure swerve drive train
+                SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new, 
+                    SwerveDriveConstants.DrivetrainConstants, 
+                    SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
+                    SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
+                );
+
+                swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
+
+                // Configure Shooter 22,23,24
+                // TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS);
+                // TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.CANIVORE_CANBUS);
+                // TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.CANIVORE_CANBUS);
+                
+                //Configure Intake 20,21
+                // SparkMax arm = new SparkMax(IntakeConstants.ARM_ID.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless);
+                // TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.RIO_CANBUS);
+                // DigitalInput armLimitSwitch = new DigitalInput(IntakeConstants.ARM_LIMIT_SWITCH_CHANNEL);
+                // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
+                // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
+                // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
+
+                // shooterIO = new ShooterReal(shooter1, shooter2, indexer);
+                // JankCoder armEncoder = new JankCoder(0, IntakeConstants.ARM_ENCODER_OFFSET);
+
+                // intakeIO = new IntakeReal(armLimitSwitch, arm, roller, armEncoder);
+                // Fault
+                FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
+
+                
+                // FaultTalonFX.addDevice(shooter1, "Shooter1");
+                // FaultTalonFX.addDevice(shooter2, "Shooter2");
+                // FaultTalonFX.addDevice(indexer, "Indexer");
+                // FaultSparkMax.addDevice(arm, "Arm");
+                // FaultTalonFX.addDevice(roller, "Roller");
+                
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
+
+                break;
+            case SIM:
+                // leftCamera = new VisionIO() {};
+                // rightCamera = new VisionIO() {};
+
+                swerveDrivetrain = new SimpleSwerveSim() {};
+
+                // shooterIO = new ShooterIO() {};
+                // intakeIO = new IntakeIO() {};
+                break;
+            default:
+                // leftCamera = new VisionIO() {};
+                // rightCamera = new VisionIO() {};
+                swerveDrivetrain = new SwerveIO() {};
+                // intakeIO = new IntakeIO() {};
+                // shooterIO = new ShooterIO() {};
+                break;
+        }
     }
 
-    void configureIntakeMotorControllers() {
+    // public class RobotMapSim {
-    }
+    //     public PhotonCameraSim leftCamera;
+    //     public PhotonCameraSim rightCamera;
+    // }
 
-    void configureDriveMotorControllers() {
+    // public RobotMapSim configureSim() {
-        // initialize SwerveModules
+    //     RobotMapSim sim = new RobotMapSim();
-        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);
+    //     // The simulated camera properties
-        this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
+    //     SimCameraProperties cameraProp = new SimCameraProperties();
-        this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
+    //     // A 640 x 480 camera with a 100 degree diagonal FOV.
-    }
+    //     cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100));
-}
+    //     // Approximate detection noise with average and standard deviation error in pixels.
+    //     cameraProp.setCalibError(0.25, 0.08);
+    //     // Set the camera image capture framerate (Note: this is limited by robot loop rate).
+    //     cameraProp.setFPS(20);
+    //     // The average and standard deviation in milliseconds of image data latency.
+    //     cameraProp.setAvgLatencyMs(35);
+    //     cameraProp.setLatencyStdDevMs(5);
+
+    //     // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
+    //     // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
+
+        
+    //     sim.leftCamera.enableRawStream(true);
+    //     sim.leftCamera.enableProcessedStream(true);
+    //     sim.leftCamera.enableDrawWireframe(true);
+
+
+    //     sim.rightCamera.enableRawStream(true);
+    //     sim.rightCamera.enableProcessedStream(true);
+    //     sim.rightCamera.enableDrawWireframe(true);
+
+    //     return sim;
+
+    // }
+   
+}

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

@@ -1,208 +0,0 @@
-// package frc4388.robot.commands.Autos;
-// import frc4388.robot.subsystems.Limelight;
-// import frc4388.robot.subsystems.SwerveDrive;
-// import frc4388.robot.Constants.AutoAlignConstants;
-// import frc4388.robot.Constants.VisionConstants;
-// import edu.wpi.first.wpilibj2.command.Command;
-
-// import edu.wpi.first.math.geometry.Translation2d;
-// import edu.wpi.first.math.geometry.Rotation2d;
-
-// import java.util.Optional;
-
-// import org.opencv.core.RotatedRect;
-
-// import edu.wpi.first.math.geometry.Pose2d;
-
-// import edu.wpi.first.wpilibj.DriverStation;
-// import edu.wpi.first.wpilibj.DriverStation.Alliance;
-
-// public class AutoAlign extends Command {
-//     private SwerveDrive swerve;
-//     private Limelight limelight;
-//     private Pose2d pose;
-//     private Translation2d targetPos;
-//     private Rotation2d targetRot;
-
-//     private Optional<Alliance> alliance;
-//     private boolean isRed;
-
-//     private boolean isFinished;
-//     private boolean isReverseFinished;
-
-//     private boolean reverseAfterFinish;
-//     private Translation2d[] moveStickReplayArr;
-//     private Translation2d[] rotStickReplayArr;
-//     private int replayIndex;
-
-//     // PID Stuff
-//     private double prevError;
-//     private double cumError;
-
-//     public AutoAlign(SwerveDrive swerve, Limelight limelight) {
-//         this.swerve = swerve;
-//         this.limelight = limelight;
-//     }
-
-//     // Calc the closest point on a circle, to the center of the speaker 
-//     private Translation2d calcTargetPos(){
-//         final double R = VisionConstants.targetPosDistance;
-//         final double cX;
-//         final double cY;
-//         if(isRed){
-//             cX = VisionConstants.RedSpeakerCenter.getX();
-//             cY = VisionConstants.RedSpeakerCenter.getY();
-//         }else{
-//             cX = VisionConstants.BlueSpeakerCenter.getX();
-//             cY = VisionConstants.BlueSpeakerCenter.getY();
-//         }
-//         final double pX = pose.getX();
-//         final double pY = pose.getY();
-
-//         // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point
-//         double vX = pX - cX;
-//         double vY = pY - cY;
-//         double magV = Math.sqrt(vX*vX + vY*vY);
-//         double aX = cX + vX / magV * R;
-//         double aY = cY + vY / magV * R;
-
-//         return new Translation2d(aX, aY);
-//     }
-
-//     // Calculate the angle facing the center, at the target rot
-//     private Rotation2d calcTargetRot() {
-//         final double R = VisionConstants.targetPosDistance;
-//         final double cX;
-//         final double cY;
-//         if(isRed){
-//             cX = VisionConstants.RedSpeakerCenter.getX();
-//             cY = VisionConstants.RedSpeakerCenter.getY();
-//         }else{
-//             cX = VisionConstants.BlueSpeakerCenter.getX();
-//             cY = VisionConstants.BlueSpeakerCenter.getY();
-//         }
-//         final double pX = pose.getX();
-//         final double pY = pose.getY();
-
-//         final double dX = pX - cX;
-//         final double dY = pY - cY;
-
-//         final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360);
-
-//         return Rotation2d.fromDegrees(yaw);
-//     }
-
-//     // Clamp to a circle, like an xbox controller
-//     private Translation2d clamp(double oldX, double oldY){
-//         // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley
-//         final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360;
-//         final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI));
-//         final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI));
-
-//         final double newX = Math.max(-maxX, Math.min(maxX, oldX));
-//         final double newY = Math.max(-maxY, Math.min(maxY, oldY));
-
-//         return new Translation2d(newX, newY);
-//     }
-
-//     private Translation2d calcMoveStick(){
-//         final double curX = pose.getX();
-//         final double curY = pose.getY();
-
-//         // I think this might work, assuming the constants are good
-//         double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed;
-//         double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed;
-
-//         return clamp(stickX, stickY);
-//     }
-
-//     private Translation2d calcRotStick(){
-//         double error = pose.getRotation().getDegrees() - targetRot.getDegrees();
-// 		cumError += error * .02; // 20 ms
-// 		double delta = error - prevError;
-
-//         final double kP = 4;
-//         final double kI = 4;
-//         final double kD = 4;
-//         final double kF = 4;
-
-// 		double output = error * kP;
-// 		output += cumError * kI;
-// 		output += delta * kD;
-// 		output += kF;
-
-//         prevError = error;
-//         return clamp(output, 0);
-//     }
-
-//     public void reverse() {
-//         this.reverseAfterFinish = true;
-//     }
-
-//     // Called when the command is initially scheduled.
-// 	@Override 
-// 	public final void initialize() {
-//         isRed = alliance.get() == DriverStation.Alliance.Red;
-//         if(reverseAfterFinish){
-//             // isReverseFinished = false;
-//             replayIndex = 0;
-//         }else{
-//             moveStickReplayArr = new Translation2d[]{};
-//             rotStickReplayArr = new Translation2d[]{};
-//         }
-// 	}
-
-//     // Called every time the scheduler runs while the command is scheduled.
-// 	@Override
-//     public void execute() {
-//         // Update limelight pos
-//         pose = limelight.getPose();
-
-//         // These must be 0, or it will error
-//         Translation2d moveStick = new Translation2d(0, 0);
-//         Translation2d rotStick = new Translation2d(0, 0);
-
-//         // Regular replay
-//         if(!isFinished){
-//             targetPos = calcTargetPos();
-//             targetRot = calcTargetRot();
-
-//             moveStick = calcMoveStick();
-//             rotStick = calcRotStick();
-
-//             // This is a way of appending...
-//             moveStickReplayArr[moveStickReplayArr.length] = moveStick;
-//             rotStickReplayArr[rotStickReplayArr.length] = rotStick;
-
-//             // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){
-//             //     replayIndex
-//             // }
-//             isFinished = limelight.isNearSpeaker();
-
-//         // If reverseAfterFinish, then loop back over and replay in reverse
-//         }else if(reverseAfterFinish && !isReverseFinished){
-//             // Get reverse direction
-//             moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1];
-//             rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1];
-
-//             // Invert sticks
-//             moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1);
-//             rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1);
-
-//             replayIndex++;
-
-//             if(replayIndex >= moveStickReplayArr.length){
-//                 reverseAfterFinish = true;
-//             }
-//         }
-
-//         // This would greatly benifit from having feild Relative implemented.
-//         swerve.driveWithInput(moveStick, rotStick, true);
-//     }
-
-//     // Returns true when the command should end.
-// 	@Override
-// 	public final boolean isFinished() {
-//         return isFinished && (isReverseFinished || !reverseAfterFinish);
-// 	}
-// }

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/neo AutoRecoding format.txt

@@ -1,20 +0,0 @@
-AUTO file format
-
-HEADER static size 0x5
-0x00 BYTE   NUM AXES: defualts to 6
-0x01 BYTE   NUM POV: defualts to 1
-0x02 BYTE   NUM CONTROLLERS: defualts to 2
-0x03 SHORT  FRAMES: any value greator or equal than one.
-
-FRAME PER CONTROLLER: defualt size 0x34
-0x00 DOUBLE AXES[NUM AXES]
-0x30 SHORT  BUTTONS
-0x32 SHORT  POVs[NUM POV]
-
-FRAME: size varrys
-FRAME PER CONTROLLER[NUM CONTROLLERS]
-INT         UNIXTIMESTAMP
-
-FILE:
-HEADER
-FRAME[FRAMES]

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

@@ -1,107 +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.commands.Swerve.neoJoystickPlayback;
-// import frc4388.robot.subsystems.SwerveDrive;
-// import frc4388.utility.controller.VirtualController;
-
-// public class neoPlaybackChooser {
-//     private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();    
-//     private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
-//     private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
-
-//     private final SwerveDrive m_swerve;
-//     private final VirtualController[] m_controllers;
-//     // 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;
-
-//     // commands
-//     private Command m_noAuto = new InstantCommand();
-    
-//     public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
-//         m_teamChosser.addOption("Red", "red");
-//         m_teamChosser.setDefaultOption("Blue", "blue");
-//         m_teamChosser.addOption("Nuetral", "nuetral");
-//         m_possitionChosser.addOption("AMP", "amp");
-//         m_possitionChosser.setDefaultOption("Center", "center");
-//         m_possitionChosser.addOption("Source", "source");
-//         m_swerve = swerve;
-//         m_controllers = controllers;
-//     }
-
-//     public neoPlaybackChooser addOption(String name, String option) {
-//         m_autoNameChosser.addOption(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 render() {
-//         // var chooser = new SendableChooser<Command>();
-//         // // chooser.setDefaultOption("No Auto", m_noAuto);
-
-//         // m_choosers.add(chooser);
-//         ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
-//             .add("Command: " + m_choosers.size(), chooser)
-//             .withSize(4, 1)
-//             .withPosition(0, m_choosers.size() - 1)
-//             .withWidget(BuiltInWidgets.kSplitButtonChooser)
-//             .withWidget(BuiltInWidgets.kComboBoxChooser);
-
-
-//         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 String autoName() {
-//         return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
-//     }
-
-//     public Command getCommand() {
-//         return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
-//     }
-// }

src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java

@@ -1,52 +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.Intake;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Intake;
-import frc4388.robot.subsystems.Shooter;
-
-public class ArmIntakeIn extends Command {
-  /** Creates a new ArmIntakeIn. */
-  private Intake robotIntake;
-  private Shooter robotShooter;
-
-  public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    this.robotIntake = robotIntake;
-    this.robotShooter = robotShooter;
-
-    addRequirements(this.robotIntake, this.robotShooter);
-  }
-
-  // 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() {
-    robotIntake.PIDOut();
-    robotIntake.spinIntakeMotor();
-  }
-
-  // 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 robotIntake.getIntakeLimitSwitchState();
-    // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) 
-    // {
-    //   return !true==true;
-    // } 
-    // else 
-    // {
-    //   return !false==!(!(true));
-    // }
-  }
-}

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

@@ -1,35 +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 edu.wpi.first.math.geometry.Translation2d;
-import frc4388.robot.commands.PID;
-import frc4388.robot.subsystems.SwerveDrive;
-
-public class RotateToAngle extends PID {
-
-  SwerveDrive drive;
-  double targetAngle;
-
-  /** Creates a new RotateToAngle. */
-  public RotateToAngle(SwerveDrive drive, double targetAngle) {
-    super(0.3, 0.0, 0.0, 0.0, 1);
-    
-    this.drive = drive;
-    this.targetAngle = targetAngle;
-
-    addRequirements(drive);
-  }
-
-  @Override
-  public double getError() {
-    return targetAngle - drive.getGyroAngle();
-  }
-
-  @Override
-  public void runWithOutput(double output) {
-    drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);    
-  }
-}

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

@@ -0,0 +1,54 @@
+// 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 org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.AutoLogOutput;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PID;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+public class StayInPosition extends PID {
+    SwerveDrive drive;
+    Translation2d driveTranslation = new Translation2d();
+
+    public StayInPosition(SwerveDrive drive) {
+        super(0.3, 0.0, 0.0, 0.0, 1);
+        this.drive = drive;
+        addRequirements(drive);
+    }
+
+    public void goToTargetPose(Pose2d targetPose) {
+        Pose2d currentPose = drive.getCurrentPose();
+        double translationX = targetPose.getX() - currentPose.getX();
+        double translationY = targetPose.getY() - currentPose.getY();
+        if (translationX > 0.2){
+        //If below is changed make this change too so it never goes over 1
+          translationX = 0.2;
+        }
+        if (translationY > 0.2){
+        //Same here
+          translationY = 0.2;
+        }
+        if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) {
+            driveTranslation = new Translation2d();
+        } else {
+            //TODO: Tweak for best corrector against another bot
+            driveTranslation = new Translation2d(translationX * 4.5, translationY * 4.5); 
+        }
+
+        drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation());
+    }
+
+    @Override
+    public double getError() {
+        return 0;
+    }
+
+    @Override
+    public void runWithOutput(double output) {}
+}

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

@@ -1,197 +0,0 @@
-package frc4388.robot.commands.Swerve;
-
-import java.io.FileInputStream;
-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.DataUtils;
-import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
-import frc4388.utility.UtilityStructs.AutoRecordingFrame;
-import frc4388.utility.controller.VirtualController;
-
-
-/**
- * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s 
- * @author Zachary Wilke
- */
-public class neoJoystickPlayback extends Command {
-    private final SwerveDrive                   swerve;
-    private final VirtualController[]           controllers;
-    private final ArrayList<AutoRecordingFrame> frames         = new ArrayList<>();
-    private final Supplier<String>              filenameGetter;
-    private       String                        filename;
-    private       int                           frame_index    = 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
-    
-    private byte  m_numAxes = 0;
-    private byte  m_numPOVs = 0;
-    private byte  m_numControllers = 0;
-    private short m_numFrames = -1;
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. 
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     * @param shouldfree Unloads the auto on compleation or intruption.
-     * @param instantload Load the auto on object instantiation
-     */
-    public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
-        this.swerve = swerve;
-        this.filenameGetter = filenameGetter;
-        this.controllers = controllers;
-        this.m_shouldfree = shouldfree;
-
-        if (instantload) loadAuto();
-        addRequirements(this.swerve);
-    }
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param filename a String containing the name of the auto file you wish to playback. 
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     * @param shouldfree unloads the auto on compleation or intruption.
-     * @param instantload load the auto on object instantiation
-     */
-    public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
-        this(swerve, () -> filename, controllers, shouldfree, instantload);
-    }
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     */
-    public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers) {
-        this(swerve, filenameGetter, controllers, true, false);
-    }
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param filename a String containing the name of the auto file you wish to playback. 
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     */
-    public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
-        this(swerve, () -> filename, controllers, true, false);
-    }
-    
-    /**
-     * Load the auto file from disk into memory
-     * @return Returns true if loading was successful, else wise; return false
-     * @implNote if the auto is already loaded, it will return true. 
-     */
-    public boolean loadAuto() {
-        filename = filenameGetter.get();
-        try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
-            if (m_numFrames != -1 && m_numFrames == frames.size()) {
-                System.out.println("AUTOPLAYBACK: Auto Already loaded.");
-                return true;
-            }
-            
-            m_numAxes = stream.readNBytes(1)[0];
-            m_numPOVs = stream.readNBytes(1)[0];
-            m_numControllers = stream.readNBytes(1)[0];
-            m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2));
-
-            if (m_numControllers > controllers.length) {
-                System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers 
-                + " virtual controllers but only " + controllers.length + " were given");
-                return false;
-            }
-
-            for (int i = 0; i < m_numFrames; i++) {
-                AutoRecordingFrame frame = new AutoRecordingFrame();
-                for (int j = 0; j < m_numControllers; j++) {
-                    AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
-                    double[] axes = new double[m_numAxes];
-                    for (int k = 0; k < m_numAxes; k++) { // we love third level for loops.
-                        axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8));
-                    }
-                    short button = DataUtils.byteArrayToShort(stream.readNBytes(2));
-                    short[] POV = new short[m_numPOVs];
-                    for (int k = 0; k < m_numPOVs; k++) {
-                        POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2));
-                    }
-                    controllerFrame.axes = axes;
-                    controllerFrame.button = button;
-                    controllerFrame.POV = POV;
-                    frame.controllerFrames[j] = controllerFrame;
-                }
-                frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4));
-                frames.add(frame);
-            }
-
-            System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long");
-            return true;
-
-        } catch (Exception e) {
-            e.printStackTrace();
-            System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`');
-            return false;
-        }
-    }
-    
-    /**
-     * Unloads the auto.
-     */
-    public void unloadAuto() {
-        System.out.println("AUTOPLAYBACK: Auto unloaded");
-        frames.clear();
-    }
-
-    @Override
-    public void initialize() {
-        startTime = System.currentTimeMillis();
-        playbackTime = 0;
-        frame_index = 0;
-
-        m_finished = !loadAuto();
-    }
-
-    @Override
-    public void execute() {
-        if (frame_index >= m_numFrames) m_finished = true;
-        if (m_finished) return;
-
-        // if (frame_index == 0) {
-        //    startTime = System.currentTimeMillis();
-        //    playbackTime = 0;
-        // } else {
-        //    playbackTime = System.currentTimeMillis() - startTime;
-        // }
-
-        AutoRecordingFrame frame = frames.get(frame_index);
-        for (int i = 0; i < controllers.length; i++) {
-            AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i];
-            controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
-            if (i == 0) {
-                this.swerve.driveWithInput(
-                    new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
-                    new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
-                    true);
-            }
-        }
-        frame_index++;
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        for (VirtualController controller : controllers) controller.zeroControls();
-        swerve.stopModules();
-        if (m_shouldfree) unloadAuto();
-    }
-    
-    @Override
-    public boolean isFinished() {
-        return m_finished;
-    }
-}