diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json
new file mode 100644
index 00000000..b1de0b1f
--- /dev/null
+++ b/.DataLogTool/datalogtool.json
@@ -0,0 +1,7 @@
+{
+  "download": {
+    "localDir": "C:\\Users\\robot\\Downloads\\AllLogs",
+    "remoteDir": "/u/logs",
+    "serverTeam": "5010"
+  }
+}
diff --git a/.SysId/sysid.json b/.SysId/sysid.json
new file mode 100644
index 00000000..0967ef42
--- /dev/null
+++ b/.SysId/sysid.json
@@ -0,0 +1 @@
+{}
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
new file mode 100644
index 00000000..02b78d36
--- /dev/null
+++ b/.github/workflows/build.yml
@@ -0,0 +1,18 @@
+name: Build
+
+on:
+  push:
+  pull_request:
+
+jobs:
+  build:
+    name: Build
+    runs-on: ubuntu-latest
+    container: wpilib/roborio-cross-ubuntu:2024-22.04
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+      - name: Grant execute permission
+        run: chmod +x gradlew
+      - name: Build robot code
+        run: ./gradlew build
diff --git a/.gitignore b/.gitignore
index 34cbaac1..165ff422 100644
--- a/.gitignore
+++ b/.gitignore
@@ -185,3 +185,7 @@ compile_commands.json
 
 # Eclipse generated file for annotation processors
 .factorypath
+/results
+/src/main/resources/pose_optimizer
+src/main/java/frc/robot/BuildConstants.java
+src/main/java/frc/robot/BuildConstants.java
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 8328c83c..7be76ce3 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -163,5 +163,5 @@
       "url": "./src/main/resources/schemas/yams-shooter.schema.json"
     }
   ],
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
 }
diff --git a/build.gradle b/build.gradle
index d555a2bf..217d7d17 100644
--- a/build.gradle
+++ b/build.gradle
@@ -47,10 +47,18 @@ deploy {
                 // getTargetTypeClass is a shortcut to get the class type using a string
 
                 frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                    // Heap: pre-size to 32m so the JVM doesn't start with a tiny default heap
+                    // and immediately GC. Max 128m keeps us well within the roboRIO's ~400m
+                    // available to user processes.
+                    jvmArgs.add("-Xms32m")
+                    jvmArgs.add("-Xmx128m")
                     jvmArgs.add("-XX:+UnlockExperimentalVMOptions")
-                    jvmArgs.add("-XX:GCTimeRatio=5")
+                    // GCTimeRatio=5 means 1/(1+5)=16% of CPU time is allowed for GC.
+                    // Increase to 19 (≈5%) to starve GC less on a real-time loop.
+                    jvmArgs.add("-XX:GCTimeRatio=19")
                     jvmArgs.add("-XX:+UseSerialGC")
-                    jvmArgs.add("-XX:MaxGCPauseMillis=50")
+                    // Keep MaxGCPauseMillis — hint to GC to try to stay under 4ms.
+                    jvmArgs.add("-XX:MaxGCPauseMillis=4")
                     // Enable VisualVM connection
                     jvmArgs.add("-Dcom.sun.management.jmxremote=true")
                     jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
diff --git a/simgui-ds.json b/simgui-ds.json
index 01e79d2a..7e39eb7f 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -97,7 +97,8 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "Keyboard0",
+      "name": "Driver"
     }
   ]
 }
diff --git a/simgui.json b/simgui.json
index 2cfd91f7..777ffcb8 100644
--- a/simgui.json
+++ b/simgui.json
@@ -10,6 +10,51 @@
       }
     },
     "Other Devices": {
+      "CANcoder (v6)[16]": {
+        "header": {
+          "open": true
+        }
+      },
+      "SPARK MAX [10]": {
+        "header": {
+          "open": true
+        }
+      },
+      "SPARK MAX [11]": {
+        "header": {
+          "open": true
+        }
+      },
+      "SPARK MAX [11] RELATIVE ENCODER": {
+        "header": {
+          "open": true
+        }
+      },
+      "SPARK MAX [12]": {
+        "header": {
+          "open": true
+        }
+      },
+      "SPARK MAX [13]": {
+        "header": {
+          "open": true
+        }
+      },
+      "SPARK MAX [13] RELATIVE ENCODER": {
+        "header": {
+          "open": true
+        }
+      },
+      "SPARK MAX [16]": {
+        "header": {
+          "open": true
+        }
+      },
+      "SPARK MAX [16] RELATIVE ENCODER": {
+        "header": {
+          "open": true
+        }
+      },
       "SPARK MAX [5]": {
         "header": {
           "open": true
@@ -20,6 +65,11 @@
           "open": true
         }
       },
+      "SPARK MAX [9]": {
+        "header": {
+          "open": true
+        }
+      },
       "Talon FX (v6)[12]": {
         "header": {
           "open": true
@@ -64,6 +114,9 @@
         "header": {
           "open": true
         }
+      },
+      "window": {
+        "visible": false
       }
     }
   },
@@ -86,8 +139,10 @@
       "/Shuffleboard/BabySwerve/Auto Modes": "String Chooser",
       "/Shuffleboard/ExampleRobot/Auto Modes": "String Chooser",
       "/SmartDashboard/Alerts": "Alerts",
+      "/SmartDashboard/Alpha/Auto Modes": "String Chooser",
       "/SmartDashboard/Arm/mechanism": "Mechanism2d",
       "/SmartDashboard/Auto Modes": "String Chooser",
+      "/SmartDashboard/Climb/mechanism": "Mechanism2d",
       "/SmartDashboard/Drive Visual": "Mechanism2d",
       "/SmartDashboard/DrivePoseEstimator/Pose Field": "Field2d",
       "/SmartDashboard/DrivePoseEstimator/values/Pose Field": "Field2d",
@@ -98,6 +153,31 @@
       "/SmartDashboard/Hood/mechanism": "Mechanism2d",
       "/SmartDashboard/IMU": "Alerts",
       "/SmartDashboard/JSON": "Alerts",
+      "/SmartDashboard/Mechanisms/Commands/Climb/Live Tuning": "Command",
+      "/SmartDashboard/Mechanisms/Commands/ElevatorMotor/Down": "Command",
+      "/SmartDashboard/Mechanisms/Commands/ElevatorMotor/Up": "Command",
+      "/SmartDashboard/Mechanisms/Commands/ElevatorMotor/ZeroEncoder": "Command",
+      "/SmartDashboard/Mechanisms/Commands/Intake/Live Tuning": "Command",
+      "/SmartDashboard/Mechanisms/Commands/Launcher/Live Tuning": "Command",
+      "/SmartDashboard/Mechanisms/Commands/climb.json/Live Tuning": "Command",
+      "/SmartDashboard/Mechanisms/Commands/flywheelMotor/Down": "Command",
+      "/SmartDashboard/Mechanisms/Commands/flywheelMotor/Up": "Command",
+      "/SmartDashboard/Mechanisms/Commands/flywheelMotor/ZeroEncoder": "Command",
+      "/SmartDashboard/Mechanisms/Commands/hoodMotor/Down": "Command",
+      "/SmartDashboard/Mechanisms/Commands/hoodMotor/Up": "Command",
+      "/SmartDashboard/Mechanisms/Commands/hoodMotor/ZeroEncoder": "Command",
+      "/SmartDashboard/Mechanisms/Commands/hopperMotor/Down": "Command",
+      "/SmartDashboard/Mechanisms/Commands/hopperMotor/Up": "Command",
+      "/SmartDashboard/Mechanisms/Commands/hopperMotor/ZeroEncoder": "Command",
+      "/SmartDashboard/Mechanisms/Commands/lifterMotor/Down": "Command",
+      "/SmartDashboard/Mechanisms/Commands/lifterMotor/Up": "Command",
+      "/SmartDashboard/Mechanisms/Commands/lifterMotor/ZeroEncoder": "Command",
+      "/SmartDashboard/Mechanisms/Commands/spintakeMotor/Down": "Command",
+      "/SmartDashboard/Mechanisms/Commands/spintakeMotor/Up": "Command",
+      "/SmartDashboard/Mechanisms/Commands/spintakeMotor/ZeroEncoder": "Command",
+      "/SmartDashboard/Mechanisms/Commands/turretMotor/Down": "Command",
+      "/SmartDashboard/Mechanisms/Commands/turretMotor/Up": "Command",
+      "/SmartDashboard/Mechanisms/Commands/turretMotor/ZeroEncoder": "Command",
       "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/Down": "Command",
       "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/LiveTuning": "Command",
       "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/Up": "Command",
@@ -108,311 +188,268 @@
       "/SmartDashboard/PhotonAlerts": "Alerts",
       "/SmartDashboard/Pigeon 2 (v6) [13]": "Gyro",
       "/SmartDashboard/Pigeon 2 [13]": "Gyro",
+      "/SmartDashboard/Rebuilt/Auto Modes": "String Chooser",
       "/SmartDashboard/Robot Visual": "Mechanism2d",
       "/SmartDashboard/SendableChooser[0]": "String Chooser",
       "/SmartDashboard/Shooter/mechanism": "Mechanism2d",
       "/SmartDashboard/Swerve Drive": "Alerts",
+      "/SmartDashboard/TigerShark/Auto Modes": "String Chooser",
       "/SmartDashboard/Turret/mechanism": "Mechanism2d",
       "/SmartDashboard/VisionSystemSim-Vision/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d",
+      "/SmartDashboard/flywheel/mechanism": "Mechanism2d",
+      "/SmartDashboard/hood/mechanism": "Mechanism2d",
       "/SmartDashboard/hoodmotor/mechanism": "Mechanism2d",
+      "/SmartDashboard/hopper/mechanism": "Mechanism2d",
+      "/SmartDashboard/lifter/mechanism": "Mechanism2d",
+      "/SmartDashboard/lowershootermotor/mechanism": "Mechanism2d",
       "/SmartDashboard/navX-Sensor[1]": "Gyro",
-      "/SmartDashboard/navX-Sensor[4]": "Gyro"
+      "/SmartDashboard/navX-Sensor[4]": "Gyro",
+      "/SmartDashboard/pinion/mechanism": "Mechanism2d",
+      "/SmartDashboard/spintake/mechanism": "Mechanism2d",
+      "/SmartDashboard/transfer/mechanism": "Mechanism2d",
+      "/SmartDashboard/turret/mechanism": "Mechanism2d",
+      "/SmartDashboard/turretmotor/mechanism": "Mechanism2d",
+      "/SmartDashboard/uppershootermotor/mechanism": "Mechanism2d",
+      "/SmartDashboard/winch_motor/mechanism": "Mechanism2d"
     },
     "windows": {
-      "/FMSInfo": {
+      "/AdvantageKit/RealOutputs/PathPlanner": {
         "window": {
           "visible": true
         }
       },
-      "/SmartDashboard/Drive Visual": {
+      "/SmartDashboard/Auto Modes": {
         "window": {
           "visible": true
         }
       },
       "/SmartDashboard/DrivePoseEstimator/Pose Field": {
         "CARPET0": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET1": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET10": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET11": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET12": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET13": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET14": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET15": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET16": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET17": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET18": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.4000000059604645,
           "width": 0.4000000059604645
         },
         "CARPET19": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET2": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET20": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET21": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET22": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET23": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET24": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET3": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET4": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET5": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET6": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET7": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET8": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "CARPET9": {
-          "image": ".\\pictures\\carpet.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 1": {
-          "image": ".\\pictures\\AT1.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 10": {
-          "image": ".\\pictures\\AT10.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 11": {
-          "image": ".\\pictures\\AT11.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 12": {
-          "image": ".\\pictures\\AT12.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 13": {
-          "image": ".\\pictures\\AT13.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 14": {
-          "image": ".\\pictures\\AT14.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 15": {
-          "image": ".\\pictures\\AT15.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 16": {
-          "image": ".\\pictures\\AT16.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 17": {
-          "image": ".\\pictures\\AT17.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 18": {
-          "image": ".\\pictures\\AT18.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 19": {
-          "image": ".\\pictures\\AT19.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 2": {
-          "image": ".\\pictures\\AT2.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 20": {
-          "image": ".\\pictures\\AT20.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 21": {
-          "image": ".\\pictures\\AT21.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 22": {
-          "image": ".\\pictures\\AT22.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 23": {
-          "image": ".\\pictures\\AT23.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 24": {
-          "image": ".\\pictures\\AT24.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 25": {
-          "image": ".\\pictures\\AT25.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 26": {
-          "image": ".\\pictures\\AT26.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 27": {
-          "image": ".\\pictures\\AT27.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 28": {
-          "image": ".\\pictures\\AT28.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 29": {
-          "image": ".\\pictures\\AT29.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 3": {
-          "image": ".\\pictures\\AT3.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 30": {
-          "image": ".\\pictures\\AT30.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 31": {
-          "image": ".\\pictures\\AT31.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 32": {
-          "image": ".\\pictures\\AT32.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 4": {
-          "image": ".\\pictures\\AT4.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 5": {
-          "image": ".\\pictures\\AT5.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 6": {
-          "image": ".\\pictures\\AT6.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 7": {
-          "image": ".\\pictures\\AT7.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 8": {
-          "image": ".\\pictures\\AT8.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
         "Field Tag 9": {
-          "image": ".\\pictures\\AT9.png",
           "length": 0.44999998807907104,
           "width": 0.44999998807907104
         },
@@ -651,26 +688,271 @@
           "length": 0.15000000596046448,
           "width": 0.15000000596046448
         },
+        "GPA51": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA52": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA53": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA54": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA55": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA56": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA57": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA58": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA59": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
         "GPA6": {
           "image": ".\\pictures\\gpa.png",
           "length": 0.15000000596046448,
           "width": 0.15000000596046448
         },
+        "GPA60": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA61": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA62": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA63": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA64": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA65": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA66": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA67": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA68": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA69": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
         "GPA7": {
           "image": ".\\pictures\\gpa.png",
           "length": 0.15000000596046448,
           "width": 0.15000000596046448
         },
+        "GPA70": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA71": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA72": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA73": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA74": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA75": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA76": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA77": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA78": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA79": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
         "GPA8": {
           "image": ".\\pictures\\gpa.png",
           "length": 0.15000000596046448,
           "width": 0.15000000596046448
         },
+        "GPA80": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA81": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA82": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA83": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA84": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA85": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA86": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA87": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA88": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA89": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
         "GPA9": {
           "image": ".\\pictures\\gpa.png",
           "length": 0.15000000596046448,
           "width": 0.15000000596046448
         },
+        "GPA90": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA91": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA92": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA93": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA94": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA95": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA96": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA97": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA98": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
+        "GPA99": {
+          "image": ".\\pictures\\gpa.png",
+          "length": 0.15000000596046448,
+          "width": 0.15000000596046448
+        },
         "GPB0": {
           "length": 0.4000000059604645,
           "width": 0.4000000059604645
@@ -786,7 +1068,6 @@
           ]
         },
         "Robot": {
-          "image": ".\\pictures\\robot.png",
           "length": 0.8669999837875366,
           "width": 0.8669999837875366
         },
@@ -811,11 +1092,6 @@
           "visible": true
         }
       },
-      "/SmartDashboard/ExampleRobot/Auto Modes": {
-        "window": {
-          "visible": true
-        }
-      },
       "/SmartDashboard/Field": {
         "OdometryPose": {
           "arrowColor": [
@@ -841,14 +1117,7 @@
         "left": 245,
         "right": 3942,
         "top": 118,
-        "width": 16.54119300842285
-      },
-      "/SmartDashboard/Robot Visual": {
-        "window": {
-          "visible": true
-        }
-      },
-      "/SmartDashboard/Shooter/mechanism": {
+        "width": 16.54119300842285,
         "window": {
           "visible": true
         }
@@ -864,7 +1133,22 @@
           "visible": true
         }
       },
-      "/SmartDashboard/hoodmotor/mechanism": {
+      "/SmartDashboard/flywheel/mechanism": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/hood/mechanism": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/hopper/mechanism": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/turret/mechanism": {
         "window": {
           "visible": true
         }
@@ -896,6 +1180,15 @@
         "Drive": {
           "open": true
         },
+        "Indexer": {
+          "open": true
+        },
+        "Intake": {
+          "open": true
+        },
+        "Launcher": {
+          "open": true
+        },
         "Vision": {
           "Camera left": {
             "open": true
@@ -932,9 +1225,21 @@
             }
           }
         },
+        "flywheel": {
+          "open": true
+        },
+        "hopper": {
+          "hopperMotor": {
+            "open": true
+          },
+          "open": true
+        },
         "open": true
       },
       "Shuffleboard": {
+        ".metadata": {
+          "open": true
+        },
         "ExampleSubsystem": {
           "percent_motor": {
             "open": true
@@ -953,6 +1258,9 @@
           },
           "open": true
         },
+        "ClimbCommands": {
+          "open": true
+        },
         "Drive Feedforward": {
           "open": true
         },
@@ -969,9 +1277,33 @@
             "open": true
           }
         },
+        "Indexer": {
+          "open": true,
+          "spindexer": {
+            "open": true
+          },
+          "transfer_back": {
+            "open": true
+          },
+          "transfer_front": {
+            "open": true
+          }
+        },
         "Wheel Radius Characterization": {
           "open": true
         },
+        "hood": {
+          "open": true
+        },
+        "hopper": {
+          "mechanism": {
+            "hopperRoot": {
+              "open": true
+            },
+            "open": true
+          },
+          "open": true
+        },
         "open": true
       },
       "Tuning": {
@@ -999,5 +1331,15 @@
         }
       }
     }
+  },
+  "NetworkTables Info": {
+    "Connections": {
+      "open": true
+    },
+    "Server": {
+      "Subscribers": {
+        "open": true
+      }
+    }
   }
 }
diff --git a/src/main/deploy/baby_swerve/cameras.json b/src/main/deploy/baby_swerve/cameras.json
deleted file mode 100644
index bfdc48b6..00000000
--- a/src/main/deploy/baby_swerve/cameras.json
+++ /dev/null
@@ -1,3 +0,0 @@
-{
-  "cameras": []
-}
diff --git a/src/main/deploy/baby_swerve/cameras/localization.json b/src/main/deploy/baby_swerve/cameras/localization.json
deleted file mode 100644
index 50c5f575..00000000
--- a/src/main/deploy/baby_swerve/cameras/localization.json
+++ /dev/null
@@ -1,13 +0,0 @@
-{
-  "name": "two",
-  "use": "apriltag",
-  "type": "limelight",
-  "strategy": "LOWEST_AMBIGUITY",
-  "column": 0,
-  "x": 0.16,
-  "y": -0.1,
-  "z": 0.335,
-  "roll": -2,
-  "pitch": 0,
-  "yaw": 0
-}
diff --git a/src/main/deploy/baby_swerve/controllers.json b/src/main/deploy/baby_swerve/controllers.json
deleted file mode 100644
index 901f9478..00000000
--- a/src/main/deploy/baby_swerve/controllers.json
+++ /dev/null
@@ -1,5 +0,0 @@
-{
-  "controllers": [
-    "driver.json"
-  ]
-}
diff --git a/src/main/deploy/baby_swerve/demo_mode.json b/src/main/deploy/baby_swerve/demo_mode.json
deleted file mode 100644
index 967ff73b..00000000
--- a/src/main/deploy/baby_swerve/demo_mode.json
+++ /dev/null
@@ -1,6 +0,0 @@
-{
-  "maxSpeed": 0.5,
-  "maxAngularSpeed": 1.5,
-  "maxAccelleration": 0.5,
-  "maxAngularAccelleration": 1.5
-}
diff --git a/src/main/deploy/baby_swerve/robot.json b/src/main/deploy/baby_swerve/robot.json
deleted file mode 100644
index 67726cd4..00000000
--- a/src/main/deploy/baby_swerve/robot.json
+++ /dev/null
@@ -1,13 +0,0 @@
-{
-  "userConfig": "demo_mode.json",
-  "driveType": "YAGSL_SWERVE_DRIVE",
-  "trackWidth": 7.5,
-  "trackWidthUom": "in",
-  "wheelBase": 7.5,
-  "wheelBaseUom": "in",
-  "wheelDiameter": 3.1242,
-  "wheelDiameterUom": "in",
-  "physicalMaxSpeed": 5.93,
-  "physicalMaxSpeedUom": "m/s",
-  "driveMotorGearRatio": 1.0
-}
diff --git a/src/main/deploy/baby_swerve/yagsl_drivetrain.json b/src/main/deploy/baby_swerve/yagsl_drivetrain.json
deleted file mode 100644
index c75b4305..00000000
--- a/src/main/deploy/baby_swerve/yagsl_drivetrain.json
+++ /dev/null
@@ -1,10 +0,0 @@
-{
-  "directory": "baby_swerve/yagsl_swerve",
-  "turningMotorGearRatio": 1.0,
-  "driveModules": [
-    "frontleft.json",
-    "frontright.json",
-    "backleft.json",
-    "backright.json"
-  ]
-}
diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/controllerproperties.json b/src/main/deploy/baby_swerve/yagsl_swerve/controllerproperties.json
deleted file mode 100644
index c5ab6446..00000000
--- a/src/main/deploy/baby_swerve/yagsl_swerve/controllerproperties.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
-  "angleJoystickRadiusDeadband": 0.5,
-  "heading": {
-    "p": 0.4,
-    "i": 0,
-    "d": 0.01
-  }
-}
diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backleft.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/backleft.json
deleted file mode 100644
index 929f3fc9..00000000
--- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backleft.json
+++ /dev/null
@@ -1,27 +0,0 @@
-{
-  "drive": {
-    "type": "neo",
-    "id": 3,
-    "canbus": null
-  },
-  "angle": {
-    "type": "neo550",
-    "id": 4,
-    "canbus": null
-  },
-  "encoder": {
-    "type": "thrifty",
-    "id": 1,
-    "canbus": null
-  },
-  "inverted": {
-    "drive": false,
-    "angle": true
-  },
-  "absoluteEncoderOffset": -120.6,
-  "absoluteEncoderInverted": true,
-  "location": {
-    "front": -3.75,
-    "left": 3.75
-  }
-}
diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backright.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/backright.json
deleted file mode 100644
index a985764b..00000000
--- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backright.json
+++ /dev/null
@@ -1,27 +0,0 @@
-{
-  "drive": {
-    "type": "neo",
-    "id": 5,
-    "canbus": null
-  },
-  "angle": {
-    "type": "neo550",
-    "id": 6,
-    "canbus": null
-  },
-  "encoder": {
-    "type": "thrifty",
-    "id": 2,
-    "canbus": null
-  },
-  "inverted": {
-    "drive": false,
-    "angle": true
-  },
-  "absoluteEncoderOffset": -166.15,
-  "absoluteEncoderInverted": true,
-  "location": {
-    "front": -3.75,
-    "left": -3.75
-  }
-}
diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontleft.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontleft.json
deleted file mode 100644
index 517bdf23..00000000
--- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontleft.json
+++ /dev/null
@@ -1,27 +0,0 @@
-{
-  "drive": {
-    "type": "neo",
-    "id": 1,
-    "canbus": null
-  },
-  "angle": {
-    "type": "neo550",
-    "id": 2,
-    "canbus": null
-  },
-  "encoder": {
-    "type": "thrifty",
-    "id": 0,
-    "canbus": null
-  },
-  "inverted": {
-    "drive": false,
-    "angle": true
-  },
-  "absoluteEncoderOffset": -165.75,
-  "absoluteEncoderInverted": true,
-  "location": {
-    "front": 3.75,
-    "left": 3.75
-  }
-}
diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontright.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontright.json
deleted file mode 100644
index 7234a3ef..00000000
--- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontright.json
+++ /dev/null
@@ -1,27 +0,0 @@
-{
-  "drive": {
-    "type": "neo",
-    "id": 7,
-    "canbus": null
-  },
-  "angle": {
-    "type": "neo550",
-    "id": 8,
-    "canbus": null
-  },
-  "encoder": {
-    "type": "thrifty",
-    "id": 3,
-    "canbus": null
-  },
-  "inverted": {
-    "drive": false,
-    "angle": true
-  },
-  "absoluteEncoderOffset": -358.5,
-  "absoluteEncoderInverted": true,
-  "location": {
-    "front": 3.75,
-    "left": -3.75
-  }
-}
diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/physicalproperties.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/physicalproperties.json
deleted file mode 100644
index 2f0bd03d..00000000
--- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/physicalproperties.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
-  "conversionFactors": {
-    "angle": {
-      "gearRatio": 55.965,
-      "factor": 0
-    },
-    "drive": {
-      "gearRatio": 5.25,
-      "diameter": 3.1242,
-      "factor": 0
-    }
-  },
-  "currentLimit": {
-    "drive": 40,
-    "angle": 20
-  },
-  "rampRate": {
-    "drive": 0.1,
-    "angle": 0.1
-  },
-  "wheelGripCoefficientOfFriction": 1.19,
-  "optimalVoltage": 12,
-  "robotMass": 47.8
-}
diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/pidfproperties.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/pidfproperties.json
deleted file mode 100644
index a0d528dc..00000000
--- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/pidfproperties.json
+++ /dev/null
@@ -1,16 +0,0 @@
-{
-  "drive": {
-    "p": 0.00057373,
-    "i": 0,
-    "d": 0,
-    "f": 0,
-    "iz": 0
-  },
-  "angle": {
-    "p": 0.01125,
-    "i": 0.000002,
-    "d": 0,
-    "f": 0,
-    "iz": 5
-  }
-}
diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/swervedrive.json b/src/main/deploy/baby_swerve/yagsl_swerve/swervedrive.json
deleted file mode 100644
index 5fb01c6b..00000000
--- a/src/main/deploy/baby_swerve/yagsl_swerve/swervedrive.json
+++ /dev/null
@@ -1,14 +0,0 @@
-{
-  "imu": {
-    "type": "navx",
-    "id": 0,
-    "canbus": null
-  },
-  "invertedIMU": false,
-  "modules": [
-    "frontleft.json",
-    "frontright.json",
-    "backleft.json",
-    "backright.json"
-  ]
-}
diff --git a/src/main/deploy/basic_robot/cameras/intake.json b/src/main/deploy/basic_robot/cameras/intake.json
deleted file mode 100644
index 33938add..00000000
--- a/src/main/deploy/basic_robot/cameras/intake.json
+++ /dev/null
@@ -1,13 +0,0 @@
-{
-  "name": "intake",
-  "use": "target",
-  "type": "limelight",
-  "column": 0,
-  "x": 0.15,
-  "y": 0.05,
-  "z": 0.2,
-  "roll": 0,
-  "pitch": 10,
-  "yaw": 0,
-  "targetHeight": 0.02
-}
diff --git a/src/main/deploy/basic_robot/cameras/left.json b/src/main/deploy/basic_robot/cameras/left.json
deleted file mode 100644
index 0eaef281..00000000
--- a/src/main/deploy/basic_robot/cameras/left.json
+++ /dev/null
@@ -1,12 +0,0 @@
-{
-  "name": "left",
-  "use": "apriltag",
-  "type": "photonvision",
-  "column": 0,
-  "x": -0.33,
-  "y": 0.33,
-  "z": 0.5,
-  "roll": 0,
-  "pitch": 0,
-  "yaw": 90
-}
diff --git a/src/main/deploy/basic_robot/cameras/localization.json b/src/main/deploy/basic_robot/cameras/localization.json
deleted file mode 100644
index 481de59d..00000000
--- a/src/main/deploy/basic_robot/cameras/localization.json
+++ /dev/null
@@ -1,12 +0,0 @@
-{
-  "name": "localization",
-  "use": "apriltag",
-  "type": "limelight",
-  "column": 0,
-  "x": -0.35,
-  "y": -0.25,
-  "z": 0.5,
-  "roll": 0,
-  "pitch": -20,
-  "yaw": 180
-}
diff --git a/src/main/deploy/basic_robot/cameras/right.json b/src/main/deploy/basic_robot/cameras/right.json
deleted file mode 100644
index aed9a771..00000000
--- a/src/main/deploy/basic_robot/cameras/right.json
+++ /dev/null
@@ -1,12 +0,0 @@
-{
-  "name": "right",
-  "use": "apriltag",
-  "type": "photonvision",
-  "column": 0,
-  "x": -0.33,
-  "y": -0.33,
-  "z": 0.5,
-  "roll": 0,
-  "pitch": 0,
-  "yaw": -90
-}
diff --git a/src/main/deploy/basic_robot/cameras/shooter.json b/src/main/deploy/basic_robot/cameras/shooter.json
deleted file mode 100644
index 9cf3bb35..00000000
--- a/src/main/deploy/basic_robot/cameras/shooter.json
+++ /dev/null
@@ -1,20 +0,0 @@
-{
-  "name": "shooter",
-  "use": "target",
-  "type": "photonvision",
-  "column": 0,
-  "x": 0.25,
-  "y": 0,
-  "z": 0.25,
-  "roll": 0,
-  "pitch": 10,
-  "yaw": 0,
-  "targetFiducialIds": [
-    17,
-    18,
-    19,
-    20,
-    21,
-    22
-  ]
-}
diff --git a/src/main/deploy/basic_robot/competition_mode.json b/src/main/deploy/basic_robot/competition_mode.json
deleted file mode 100644
index f67b949b..00000000
--- a/src/main/deploy/basic_robot/competition_mode.json
+++ /dev/null
@@ -1,6 +0,0 @@
-{
-  "maxSpeed": 6.0,
-  "maxAngularSpeed": 6.0,
-  "maxAccelleration": 3.0,
-  "maxAngularAccelleration": 18.75
-}
diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_left_trigger.json b/src/main/deploy/basic_robot/controllers/axis/driver_left_trigger.json
deleted file mode 100644
index 41b79133..00000000
--- a/src/main/deploy/basic_robot/controllers/axis/driver_left_trigger.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
-  "channel": 2,
-  "deadband": 0.07,
-  "invert": true,
-  "scale": 1.0,
-  "curvePower": 3,
-  "limit": 1.0
-}
diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_left_x.json b/src/main/deploy/basic_robot/controllers/axis/driver_left_x.json
deleted file mode 100644
index d77f0e91..00000000
--- a/src/main/deploy/basic_robot/controllers/axis/driver_left_x.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
-  "channel": 0,
-  "deadband": 0.07,
-  "invert": true,
-  "scale": 1.0,
-  "curvePower": 3,
-  "limit": 1.0
-}
diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_left_y.json b/src/main/deploy/basic_robot/controllers/axis/driver_left_y.json
deleted file mode 100644
index d826731e..00000000
--- a/src/main/deploy/basic_robot/controllers/axis/driver_left_y.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
-  "channel": 1,
-  "deadband": 0.07,
-  "invert": true,
-  "scale": 1.0,
-  "curvePower": 3.0,
-  "limit": 1.0
-}
diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_right_trigger.json b/src/main/deploy/basic_robot/controllers/axis/driver_right_trigger.json
deleted file mode 100644
index 47b46917..00000000
--- a/src/main/deploy/basic_robot/controllers/axis/driver_right_trigger.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
-  "channel": 3,
-  "deadband": 0.07,
-  "invert": true,
-  "scale": 1.0,
-  "curvePower": 3,
-  "limit": 1.0
-}
diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_right_x.json b/src/main/deploy/basic_robot/controllers/axis/driver_right_x.json
deleted file mode 100644
index 8f981e23..00000000
--- a/src/main/deploy/basic_robot/controllers/axis/driver_right_x.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
-  "channel": 4,
-  "deadband": 0.07,
-  "invert": true,
-  "scale": 1.0,
-  "curvePower": 3,
-  "limit": 1.0
-}
diff --git a/src/main/deploy/basic_robot/controllers/axis/operator_left_y.json b/src/main/deploy/basic_robot/controllers/axis/operator_left_y.json
deleted file mode 100644
index c13bd0e0..00000000
--- a/src/main/deploy/basic_robot/controllers/axis/operator_left_y.json
+++ /dev/null
@@ -1,9 +0,0 @@
-{
-  "channel": 1,
-  "deadband": 0.07,
-  "invert": "true",
-  "scale": 1.0,
-  "curvePower": 3.0,
-  "limit": 1.0,
-  "rate": 1.0
-}
diff --git a/src/main/deploy/basic_robot/controllers/axis/operator_right_y.json b/src/main/deploy/basic_robot/controllers/axis/operator_right_y.json
deleted file mode 100644
index 2668956f..00000000
--- a/src/main/deploy/basic_robot/controllers/axis/operator_right_y.json
+++ /dev/null
@@ -1,9 +0,0 @@
-{
-  "channel": 5,
-  "deadband": 0.07,
-  "invert": "true",
-  "scale": 1.0,
-  "curvePower": 3.0,
-  "limit": 1.0,
-  "rate": 1.0
-}
diff --git a/src/main/deploy/basic_robot/controllers/driver.json b/src/main/deploy/basic_robot/controllers/driver.json
deleted file mode 100644
index e2dbc76a..00000000
--- a/src/main/deploy/basic_robot/controllers/driver.json
+++ /dev/null
@@ -1,11 +0,0 @@
-{
-  "name": "driver",
-  "port": 0,
-  "axis": [
-    "driver_left_y.json",
-    "driver_left_x.json",
-    "driver_right_x.json",
-    "driver_left_trigger.json",
-    "driver_right_trigger.json"
-  ]
-}
diff --git a/src/main/deploy/basic_robot/controllers/operator.json b/src/main/deploy/basic_robot/controllers/operator.json
deleted file mode 100644
index 858cfea8..00000000
--- a/src/main/deploy/basic_robot/controllers/operator.json
+++ /dev/null
@@ -1,8 +0,0 @@
-{
-  "name": "operator",
-  "port": 1,
-  "axis": [
-    "operator_left_y.json",
-    "operator_right_y.json"
-  ]
-}
diff --git a/src/main/deploy/basic_robot/drive_modules/backleft.json b/src/main/deploy/basic_robot/drive_modules/backleft.json
deleted file mode 100644
index c62ce932..00000000
--- a/src/main/deploy/basic_robot/drive_modules/backleft.json
+++ /dev/null
@@ -1,5 +0,0 @@
-{
-  "s": 0.19542,
-  "v": 2.2924,
-  "a": 0.35934
-}
diff --git a/src/main/deploy/basic_robot/drive_modules/backright.json b/src/main/deploy/basic_robot/drive_modules/backright.json
deleted file mode 100644
index aee7fd95..00000000
--- a/src/main/deploy/basic_robot/drive_modules/backright.json
+++ /dev/null
@@ -1,5 +0,0 @@
-{
-  "s": 0.18096,
-  "v": 2.2915,
-  "a": 0.37156
-}
diff --git a/src/main/deploy/basic_robot/drive_modules/frontleft.json b/src/main/deploy/basic_robot/drive_modules/frontleft.json
deleted file mode 100644
index bdf7d3fa..00000000
--- a/src/main/deploy/basic_robot/drive_modules/frontleft.json
+++ /dev/null
@@ -1,5 +0,0 @@
-{
-  "s": 0.21848,
-  "v": 2.3118,
-  "a": 0.20314
-}
diff --git a/src/main/deploy/basic_robot/drive_modules/frontright.json b/src/main/deploy/basic_robot/drive_modules/frontright.json
deleted file mode 100644
index 36a1c209..00000000
--- a/src/main/deploy/basic_robot/drive_modules/frontright.json
+++ /dev/null
@@ -1,5 +0,0 @@
-{
-  "s": 0.18434,
-  "v": 2.3018,
-  "a": 0.30992
-}
diff --git a/src/main/deploy/basic_robot/subsystems/example.json b/src/main/deploy/basic_robot/subsystems/example.json
deleted file mode 100644
index 91fb54bd..00000000
--- a/src/main/deploy/basic_robot/subsystems/example.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
-  "devices": [
-    {
-      "device": "percent_motor",
-      "file": "example/percent_motor.json"
-    },
-    {
-      "device": "velocity_motor",
-      "file": "example/velocity_motor.json"
-    },
-    {
-      "device": "yams_shooter",
-      "file": "example/yams_shooter.json"
-    },
-    {
-      "device": "yams_arm",
-      "file": "example/yams_arm.json"
-    },
-    {
-      "device": "yams_turret",
-      "file": "example/yams_pivot.json"
-    }
-  ]
-}
diff --git a/src/main/deploy/basic_robot/subsystems/example/velocity_motor.json b/src/main/deploy/basic_robot/subsystems/example/velocity_motor.json
deleted file mode 100644
index 185de2e0..00000000
--- a/src/main/deploy/basic_robot/subsystems/example/velocity_motor.json
+++ /dev/null
@@ -1,18 +0,0 @@
-{
-  "name": "velocity_motor",
-  "controller": "spark",
-  "type": "Neo",
-  "id": 14,
-  "gearing": 1.0,
-  "momentOfInertiaKgMSq": 1.0,
-  "x": -0.25,
-  "y": 0.0,
-  "z": 0.25,
-  "kP": 0.1,
-  "kI": 0.0,
-  "kD": 0.01,
-  "iZone": 0.0,
-  "kS": 0.1,
-  "kV": 0.1,
-  "kA": 0.0
-}
diff --git a/src/main/deploy/music/mariachi.chrp b/src/main/deploy/music/mariachi.chrp
new file mode 100644
index 00000000..a7c16056
Binary files /dev/null and b/src/main/deploy/music/mariachi.chrp differ
diff --git a/src/main/deploy/music/raiders.chrp b/src/main/deploy/music/raiders.chrp
new file mode 100644
index 00000000..7103ee9c
Binary files /dev/null and b/src/main/deploy/music/raiders.chrp differ
diff --git a/src/main/deploy/music/sea2.chrp b/src/main/deploy/music/sea2.chrp
new file mode 100644
index 00000000..e0f1a421
Binary files /dev/null and b/src/main/deploy/music/sea2.chrp differ
diff --git a/src/main/deploy/pathplanner/autos/2X-TL-CTR-BUMP-DEP.auto b/src/main/deploy/pathplanner/autos/2X-TL-CTR-BUMP-DEP.auto
new file mode 100644
index 00000000..fa784b01
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/2X-TL-CTR-BUMP-DEP.auto
@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-CTLD"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CTLD-BLA"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BLA-TLS"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-QTRH"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRLD-BLA"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BLA-DEP"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Left",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/2X-TR-CTR-BUMP-DEP.auto b/src/main/deploy/pathplanner/autos/2X-TR-CTR-BUMP-DEP.auto
new file mode 100644
index 00000000..ab2e3ba9
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/2X-TR-CTR-BUMP-DEP.auto
@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTRD"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CTRD-BA"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BA-TR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-QTRLong"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTR-BUMP"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BA-DEP"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Right",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/3Shuttle-HPC.auto b/src/main/deploy/pathplanner/autos/3Shuttle-HPC.auto
new file mode 100644
index 00000000..23915620
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/3Shuttle-HPC.auto
@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-QTRH"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CTR-NWALL"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "3RD-SWIPE"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "NWALL-CLIMB"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Right",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-BL-MS-DEP-MS.auto b/src/main/deploy/pathplanner/autos/A-BL-MS-DEP-MS.auto
new file mode 100644
index 00000000..27591f90
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-BL-MS-DEP-MS.auto
@@ -0,0 +1,50 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BL-DEP"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "DEP-CL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "launcherPrep"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "AllianceSide",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-BL-SS-DEP-SS.auto b/src/main/deploy/pathplanner/autos/A-BL-SS-DEP-SS.auto
new file mode 100644
index 00000000..9499cafc
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-BL-SS-DEP-SS.auto
@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BL-DEP"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DEP-CL"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "AllianceSide",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-BL-SS-NW-MS.auto b/src/main/deploy/pathplanner/autos/A-BL-SS-NW-MS.auto
new file mode 100644
index 00000000..2dc8a1e7
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-BL-SS-NW-MS.auto
@@ -0,0 +1,50 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BL-NW"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "launcherPrep"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Left",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-BR-MS-HP-MS.auto b/src/main/deploy/pathplanner/autos/A-BR-MS-HP-MS.auto
new file mode 100644
index 00000000..eb140426
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-BR-MS-HP-MS.auto
@@ -0,0 +1,56 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BR-HP"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 5.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "HP-CR"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "AllianceSide",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-BR-SS-HP-SS-Tested.auto b/src/main/deploy/pathplanner/autos/A-BR-SS-HP-SS-Tested.auto
new file mode 100644
index 00000000..2e0f7aa9
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-BR-SS-HP-SS-Tested.auto
@@ -0,0 +1,49 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BR-HP"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP-CR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "AllianceSide",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-BR-SS-NW-MS.auto b/src/main/deploy/pathplanner/autos/A-BR-SS-NW-MS.auto
new file mode 100644
index 00000000..9b0424f4
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-BR-SS-NW-MS.auto
@@ -0,0 +1,57 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BR-NW"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 8.5
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "launcherPrep"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Right",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-C-CL-SS-Tested.auto b/src/main/deploy/pathplanner/autos/A-C-CL-SS-Tested.auto
new file mode 100644
index 00000000..4e2b2c27
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-C-CL-SS-Tested.auto
@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "C-CL"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Center",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-C-CR-SS-Tested.auto b/src/main/deploy/pathplanner/autos/A-C-CR-SS-Tested.auto
new file mode 100644
index 00000000..ba1210c8
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-C-CR-SS-Tested.auto
@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "C-CR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Center",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-C-MS-TWR-MS.auto b/src/main/deploy/pathplanner/autos/A-C-MS-TWR-MS.auto
new file mode 100644
index 00000000..5364d3e3
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-C-MS-TWR-MS.auto
@@ -0,0 +1,44 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "C-TWR"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Center",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-HP-MS-DEP-MS.auto b/src/main/deploy/pathplanner/autos/A-HP-MS-DEP-MS.auto
new file mode 100644
index 00000000..b306d7c7
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-HP-MS-DEP-MS.auto
@@ -0,0 +1,44 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BR-HP"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "launcherPrep"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "HP-DEP"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "AllianceSide",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-TL-MS-DEP-MS.auto b/src/main/deploy/pathplanner/autos/A-TL-MS-DEP-MS.auto
new file mode 100644
index 00000000..2e7710f8
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-TL-MS-DEP-MS.auto
@@ -0,0 +1,50 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-Dep"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "DEP-CL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "launcherPrep"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "AllianceSide",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-TL-QTR-DEP.auto b/src/main/deploy/pathplanner/autos/A-TL-QTR-DEP.auto
new file mode 100644
index 00000000..90682bd2
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-TL-QTR-DEP.auto
@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-CTR-QTR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Left",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-TL-SS-CTR-MS.auto b/src/main/deploy/pathplanner/autos/A-TL-SS-CTR-MS.auto
new file mode 100644
index 00000000..a440d60a
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-TL-SS-CTR-MS.auto
@@ -0,0 +1,57 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "TL-CTR"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 8.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "launcherPrep"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Left",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-TL-SS-DEP-SS.auto b/src/main/deploy/pathplanner/autos/A-TL-SS-DEP-SS.auto
new file mode 100644
index 00000000..71c3c16f
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-TL-SS-DEP-SS.auto
@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-Dep"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DEP-CL"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "AllianceSide",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-TR-CTRQ-HP.auto b/src/main/deploy/pathplanner/autos/A-TR-CTRQ-HP.auto
new file mode 100644
index 00000000..1b448e61
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-TR-CTRQ-HP.auto
@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-QTR"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTR-TR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-HP"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Right",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-TR-MS-HP-MS.auto b/src/main/deploy/pathplanner/autos/A-TR-MS-HP-MS.auto
new file mode 100644
index 00000000..72fe7539
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-TR-MS-HP-MS.auto
@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-HP"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP-CR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "AllianceSide",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-TR-SS-CTR-MS.auto b/src/main/deploy/pathplanner/autos/A-TR-SS-CTR-MS.auto
new file mode 100644
index 00000000..5d5698a0
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-TR-SS-CTR-MS.auto
@@ -0,0 +1,26 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "TR-CTR"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Right",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-TR-SS-HP-SS.auto b/src/main/deploy/pathplanner/autos/A-TR-SS-HP-SS.auto
new file mode 100644
index 00000000..c7066dff
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-TR-SS-HP-SS.auto
@@ -0,0 +1,49 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-HP"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP-CR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "AllianceSide",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Churn-Orbit Right 2 Swipe no HP.auto b/src/main/deploy/pathplanner/autos/Churn-Orbit Right 2 Swipe no HP.auto
new file mode 100644
index 00000000..bc540b3b
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Churn-Orbit Right 2 Swipe no HP.auto	
@@ -0,0 +1,67 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-QTRAngled"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTR-TRBack"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "indexerChurn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 4.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TRBack-CTR-HALF"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRH-TRBack"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Orbit",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Disrupt.auto b/src/main/deploy/pathplanner/autos/Disrupt.auto
new file mode 100644
index 00000000..4df5fe7f
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Disrupt.auto
@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightDisrupt1"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "WaitUntilIntaking"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Disrupter2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Right",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Orbit Left 1Swipe.auto b/src/main/deploy/pathplanner/autos/Orbit Left 1Swipe.auto
new file mode 100644
index 00000000..81d0109a
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Orbit Left 1Swipe.auto	
@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-QTRHLong"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRLong-TL"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Left",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Orbit Left.auto b/src/main/deploy/pathplanner/autos/Orbit Left.auto
new file mode 100644
index 00000000..7790de27
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Orbit Left.auto	
@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-QTRH"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRLong-TL"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "launcherPrep"
+                }
+              },
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.0
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-CTR-QTR"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRL-MID"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Left",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Orbit Right 1Swipe.auto b/src/main/deploy/pathplanner/autos/Orbit Right 1Swipe.auto
new file mode 100644
index 00000000..2880825c
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Orbit Right 1Swipe.auto	
@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-QTRLong"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRLong-HP"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Orbit",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Orbit Right 2 Swipe no HP.auto b/src/main/deploy/pathplanner/autos/Orbit Right 2 Swipe no HP.auto
new file mode 100644
index 00000000..46b2aade
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Orbit Right 2 Swipe no HP.auto	
@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-QTRAngled"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTR-TRBack"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TRBack-CTR-HALF"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRH-TRBack"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.5
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TRBack-CTR-HALF"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Orbit",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Orbit Right.auto b/src/main/deploy/pathplanner/autos/Orbit Right.auto
new file mode 100644
index 00000000..5ad58705
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Orbit Right.auto	
@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-QTR"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTR-TR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-HALF"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRH-HP"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Orbit",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Right 5010 Double.auto b/src/main/deploy/pathplanner/autos/Right 5010 Double.auto
new file mode 100644
index 00000000..e05bd1e3
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Right 5010 Double.auto	
@@ -0,0 +1,55 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-QTR"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTR-TR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-QTR-BR-HP"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "untested",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/TL-DIS-NWALL-DEP.auto
similarity index 58%
rename from src/main/deploy/pathplanner/autos/New Auto.auto
rename to src/main/deploy/pathplanner/autos/TL-DIS-NWALL-DEP.auto
index 268147bb..ac3634ba 100644
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ b/src/main/deploy/pathplanner/autos/TL-DIS-NWALL-DEP.auto
@@ -7,13 +7,19 @@
         {
           "type": "path",
           "data": {
-            "pathName": "New Path"
+            "pathName": "TL-DIS"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "NWALL-DEP"
           }
         }
       ]
     }
   },
   "resetOdom": true,
-  "folder": null,
+  "folder": "Left",
   "choreoAuto": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/TL-QTR-SS-QTRH-DEP-SS.auto b/src/main/deploy/pathplanner/autos/TL-QTR-SS-QTRH-DEP-SS.auto
new file mode 100644
index 00000000..ebf76766
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/TL-QTR-SS-QTRH-DEP-SS.auto
@@ -0,0 +1,74 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-QTRH"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRLong-TL"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TL-CTR-QTR"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRL-TL"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "launcherPrep"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Left",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/TR-CTR-SS-CTRH-MS-HP-SS.auto b/src/main/deploy/pathplanner/autos/TR-CTR-SS-CTRH-MS-HP-SS.auto
new file mode 100644
index 00000000..aa6de1b3
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/TR-CTR-SS-CTRH-MS-HP-SS.auto
@@ -0,0 +1,91 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeIntake"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-QTR"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTR-TR"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherLow"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TR-CTR-HALF"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "QTRH-HP"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP-SHOOT"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "launcherPrep"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Right",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/TR-DIS-NWALL.auto b/src/main/deploy/pathplanner/autos/TR-DIS-NWALL.auto
new file mode 100644
index 00000000..440a1ea5
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/TR-DIS-NWALL.auto
@@ -0,0 +1,12 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": []
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 6e83d807..660ca524 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1,9 +1,9 @@
 {
   "field_size": {
-    "x": 15.24,
-    "y": 8.25
+    "x": 16.54,
+    "y": 8.07
   },
-  "nodeSizeMeters": 0.25,
+  "nodeSizeMeters": 0.3,
   "grid": [
     [
       true,
@@ -28,47 +28,6 @@
       true,
       true,
       true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
       true,
       true,
       true,
@@ -91,45 +50,18 @@
       true,
       true,
       true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
     ],
     [
       true,
@@ -154,47 +86,6 @@
       true,
       true,
       true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
       true,
       true,
       true,
@@ -215,6 +106,22 @@
       true,
       true,
       true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
       true,
       true,
       false,
@@ -255,15 +162,6 @@
       false,
       false,
       false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
       false,
       false,
       false,
@@ -279,7 +177,12 @@
       false,
       false,
       true,
+      true
+    ],
+    [
       true,
+      true,
+      false,
       false,
       false,
       false,
@@ -289,6 +192,13 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -307,6 +217,12 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -318,9 +234,11 @@
       false,
       false,
       false,
-      false
+      true,
+      true
     ],
     [
+      true,
       true,
       false,
       false,
@@ -332,6 +250,13 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -341,9 +266,6 @@
       false,
       false,
       false,
-      true,
-      true,
-      false,
       false,
       false,
       false,
@@ -353,6 +275,12 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -363,6 +291,13 @@
       false,
       false,
       false,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
       false,
       false,
       false,
@@ -373,6 +308,13 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -381,10 +323,6 @@
       false,
       false,
       false,
-      false
-    ],
-    [
-      true,
       false,
       false,
       false,
@@ -395,6 +333,12 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -404,6 +348,12 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
       true,
       true,
       false,
@@ -444,12 +394,6 @@
       false,
       false,
       false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
       false,
       false,
       false,
@@ -462,13 +406,19 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
       false,
       false,
       false,
       false,
       false,
-      true,
-      true,
       false,
       false,
       false,
@@ -507,10 +457,6 @@
       false,
       false,
       false,
-      false
-    ],
-    [
-      true,
       false,
       false,
       false,
@@ -518,6 +464,14 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
       false,
       false,
       false,
@@ -530,8 +484,6 @@
       false,
       false,
       false,
-      true,
-      true,
       false,
       false,
       false,
@@ -570,9 +522,16 @@
       false,
       false,
       false,
-      false
+      true,
+      true,
+      true,
+      true
     ],
     [
+      true,
+      true,
+      true,
+      true,
       true,
       false,
       false,
@@ -581,597 +540,13 @@
       false,
       false,
       false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1190,6 +565,12 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1200,15 +581,32 @@
       false,
       false,
       false,
-      false
+      true,
+      true,
+      true
     ],
     [
+      true,
+      true,
+      true,
+      true,
+      true,
       true,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
       false,
       false,
       false,
@@ -1223,6 +621,13 @@
       false,
       false,
       false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       true,
       true,
       false,
@@ -1235,11 +640,30 @@
       false,
       false,
       false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1256,6 +680,14 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1263,15 +695,33 @@
       false,
       false,
       false,
-      false
+      true,
+      true,
+      true,
+      true,
+      true
     ],
     [
+      true,
+      true,
+      true,
+      true,
+      true,
       true,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1286,24 +736,50 @@
       false,
       false,
       false,
-      true,
-      true,
-      false,
-      false,
-      false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1320,21 +796,48 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
       false,
       false,
       false,
-      false
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
     ],
     [
+      true,
+      true,
+      true,
+      true,
+      true,
       true,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1349,24 +852,50 @@
       false,
       false,
       false,
-      true,
-      true,
-      false,
-      false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1383,15 +912,29 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
       false,
       false,
       false,
-      false
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
     ],
     [
+      true,
       true,
       false,
       false,
@@ -1402,18 +945,16 @@
       false,
       false,
       false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
       true,
       true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
       false,
       false,
       false,
@@ -1429,12 +970,30 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
       false,
       false,
       false,
@@ -1445,6 +1004,13 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1452,10 +1018,6 @@
       false,
       false,
       false,
-      false
-    ],
-    [
-      true,
       false,
       false,
       false,
@@ -1467,6 +1029,12 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1477,6 +1045,15 @@
       false,
       true,
       true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
       false,
       false,
       false,
@@ -1515,10 +1092,6 @@
       false,
       false,
       false,
-      false
-    ],
-    [
-      true,
       false,
       false,
       false,
@@ -1531,6 +1104,13 @@
       false,
       false,
       false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1538,8 +1118,6 @@
       false,
       false,
       false,
-      true,
-      true,
       false,
       false,
       false,
@@ -1578,16 +1156,19 @@
       false,
       false,
       false,
-      false
-    ],
-    [
-      true,
       false,
       false,
       false,
       false,
       false,
       false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1601,8 +1182,6 @@
       false,
       false,
       false,
-      true,
-      true,
       false,
       false,
       false,
@@ -1641,9 +1220,12 @@
       false,
       false,
       false,
-      false
+      true,
+      true
     ],
     [
+      true,
+      true,
       true,
       false,
       false,
@@ -1654,6 +1236,14 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
       false,
       false,
       false,
@@ -1664,8 +1254,6 @@
       false,
       false,
       false,
-      true,
-      true,
       false,
       false,
       false,
@@ -1673,6 +1261,12 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1684,6 +1278,12 @@
       false,
       false,
       false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
       false,
       false,
       false,
@@ -1694,6 +1294,13 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1704,10 +1311,6 @@
       false,
       false,
       false,
-      false
-    ],
-    [
-      true,
       false,
       false,
       false,
@@ -1716,6 +1319,12 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1727,6 +1336,10 @@
       false,
       false,
       false,
+      true,
+      true
+    ],
+    [
       true,
       true,
       false,
@@ -1739,6 +1352,14 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
       false,
       false,
       false,
@@ -1756,6 +1377,12 @@
       false,
       false,
       false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
       false,
       false,
       false,
@@ -1767,9 +1394,11 @@
       false,
       false,
       false,
-      false
+      true,
+      true
     ],
     [
+      true,
       true,
       false,
       false,
@@ -1790,13 +1419,6 @@
       false,
       false,
       false,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
       false,
       false,
       false,
@@ -1830,7 +1452,8 @@
       false,
       false,
       false,
-      false
+      true,
+      true
     ],
     [
       true,
@@ -1855,47 +1478,6 @@
       true,
       true,
       true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
       true,
       true,
       true,
@@ -1918,45 +1500,18 @@
       true,
       true,
       true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
     ],
     [
       true,
@@ -1981,47 +1536,6 @@
       true,
       true,
       true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
-    ],
-    [
       true,
       true,
       true,
@@ -2044,45 +1558,18 @@
       true,
       true,
       true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
     ]
   ]
 }
diff --git a/src/main/deploy/pathplanner/paths/3RD-SWIPE.path b/src/main/deploy/pathplanner/paths/3RD-SWIPE.path
new file mode 100644
index 00000000..802e16ad
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/3RD-SWIPE.path
@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.611485714285715,
+        "y": 2.1173428571428565
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.0154571428571435,
+        "y": 1.6897571428571414
+      },
+      "isLocked": false,
+      "linkedName": "CTR/NWALL"
+    },
+    {
+      "anchor": {
+        "x": 5.8211,
+        "y": 1.6767999999999996
+      },
+      "prevControl": {
+        "x": 5.966058035714286,
+        "y": 1.331816071428571
+      },
+      "nextControl": {
+        "x": 5.676141964285715,
+        "y": 2.0217839285714283
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.8211,
+        "y": 5.240014285714285
+      },
+      "prevControl": {
+        "x": 5.872928571428572,
+        "y": 4.643985714285713
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "YHY"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0472440944881893,
+      "rotationDegrees": 89.1641830798336
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.8764568764568764,
+      "maxWaypointRelativePos": 1.9020979020979023,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 0.27039627039627084,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 88.6677801461304
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -88.99491399474576
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BA-DEP.path b/src/main/deploy/pathplanner/paths/BA-DEP.path
new file mode 100644
index 00000000..dd7176b9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BA-DEP.path
@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.493756708407871,
+        "y": 2.517924865831842
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.493756708407871,
+        "y": 2.5179248658318425
+      },
+      "isLocked": false,
+      "linkedName": "BUMP-AFTER"
+    },
+    {
+      "anchor": {
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
+      },
+      "prevControl": {
+        "x": 2.7371377459749544,
+        "y": 0.6682289803220032
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BA-TR.path b/src/main/deploy/pathplanner/paths/BA-TR.path
new file mode 100644
index 00000000..adf2b51e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BA-TR.path
@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.493756708407871,
+        "y": 2.517924865831842
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8771914132379246,
+        "y": 1.6093023255813943
+      },
+      "isLocked": false,
+      "linkedName": "BUMP-AFTER"
+    },
+    {
+      "anchor": {
+        "x": 3.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "prevControl": {
+        "x": 2.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "trsj"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 0.010169491525423728,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BL-DEP.path b/src/main/deploy/pathplanner/paths/BL-DEP.path
new file mode 100644
index 00000000..4e4d7fa8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BL-DEP.path
@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5880826799714898,
+        "y": 6.5478581610833935
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.275751960085531,
+        "y": 6.983342836778333
+      },
+      "isLocked": false,
+      "linkedName": "BLS"
+    },
+    {
+      "anchor": {
+        "x": 0.8719949494949504,
+        "y": 7.321729797979798
+      },
+      "prevControl": {
+        "x": 1.7814694250807417,
+        "y": 7.7374895582475895
+      },
+      "nextControl": {
+        "x": 0.4344325831371458,
+        "y": 7.121701287644801
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7231186868686872,
+        "y": 5.523762626262627
+      },
+      "prevControl": {
+        "x": 0.6773106060606064,
+        "y": 6.1536237373737395
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DEPOT"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.3038596491228112,
+      "rotationDegrees": -104.27045396689256
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -114.56717132151371
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.88820165166617
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BL-NW.path b/src/main/deploy/pathplanner/paths/BL-NW.path
new file mode 100644
index 00000000..3030c39d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BL-NW.path
@@ -0,0 +1,95 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6090277777777784,
+        "y": 6.073459595959596
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.1464618476280997,
+        "y": 7.023595020050829
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.5757234497505355,
+        "y": 7.535498930862438
+      },
+      "prevControl": {
+        "x": 3.5757234497505355,
+        "y": 7.535498930862438
+      },
+      "nextControl": {
+        "x": 5.5757234497505355,
+        "y": 7.535498930862438
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.200955096222382,
+        "y": 7.06043121881682
+      },
+      "prevControl": {
+        "x": 5.88433620109375,
+        "y": 7.44740986841848
+      },
+      "nextControl": {
+        "x": 6.538503207412688,
+        "y": 6.647872416250891
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.3259729151817545,
+        "y": 1.6096543121881677
+      },
+      "prevControl": {
+        "x": 6.346060606060607,
+        "y": 2.660757575757576
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.593684210526313,
+      "rotationDegrees": 179.14780805275348
+    },
+    {
+      "waypointRelativePos": 2.153684210526314,
+      "rotationDegrees": -89.05422605441628
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -91.07082445478696
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -179.49123935164835
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BLA-DEP.path b/src/main/deploy/pathplanner/paths/BLA-DEP.path
new file mode 100644
index 00000000..fd5270d0
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BLA-DEP.path
@@ -0,0 +1,105 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.6560107334525935,
+        "y": 5.649427549194991
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.428855098389982,
+        "y": 6.57427549194991
+      },
+      "isLocked": false,
+      "linkedName": "BLA"
+    },
+    {
+      "anchor": {
+        "x": 2.0069946332737025,
+        "y": 6.9312343470483
+      },
+      "prevControl": {
+        "x": 2.211802613403463,
+        "y": 6.787868760957469
+      },
+      "nextControl": {
+        "x": 1.6723013677124379,
+        "y": 7.165519632941184
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.9847942754919494,
+        "y": 7.077262969588551
+      },
+      "prevControl": {
+        "x": 1.1632737030411446,
+        "y": 7.385545617173523
+      },
+      "nextControl": {
+        "x": 0.8548002027622305,
+        "y": 6.852727753055401
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7231186868686872,
+        "y": 5.523762626262627
+      },
+      "prevControl": {
+        "x": 0.6765116279069764,
+        "y": 6.606726296958855
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DEPOT"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 2.013368983957223,
+      "rotationDegrees": -110.4131318098436
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.5254237288135593,
+      "maxWaypointRelativePos": 3.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -114.56717132151371
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BLA-TLS.path b/src/main/deploy/pathplanner/paths/BLA-TLS.path
new file mode 100644
index 00000000..a62da52c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BLA-TLS.path
@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.6560107334525935,
+        "y": 5.649427549194991
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.6500357781753126,
+        "y": 7.207066189624329
+      },
+      "isLocked": false,
+      "linkedName": "BLA"
+    },
+    {
+      "anchor": {
+        "x": 4.35787951807229,
+        "y": 7.411662650602409
+      },
+      "prevControl": {
+        "x": 3.3578795180722896,
+        "y": 7.411662650602409
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TLS"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/BR-HP.path
similarity index 56%
rename from src/main/deploy/pathplanner/paths/New Path.path
rename to src/main/deploy/pathplanner/paths/BR-HP.path
index 38908444..ae554752 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/BR-HP.path
@@ -3,45 +3,45 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 8.031762295081966,
-        "y": 6.728227459016393
+        "x": 3.6113181818181816,
+        "y": 1.6438181818181818
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.677151639344262,
-        "y": 6.728227459016393
+        "x": 3.0612397783969407,
+        "y": 1.4937967990669336
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "BR"
     },
     {
       "anchor": {
-        "x": 4.207684426229508,
-        "y": 6.88406762295082
+        "x": 2.3003991446899503,
+        "y": 1.0595759087669274
       },
       "prevControl": {
-        "x": 5.3096147111058976,
-        "y": 7.435032765389015
+        "x": 3.2582254299110964,
+        "y": 1.346923794333272
       },
       "nextControl": {
-        "x": 3.8960040983606556,
-        "y": 6.728227459016393
+        "x": 1.3002565930149685,
+        "y": 0.7595331432644322
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.812090163934426,
-        "y": 5.217776639344262
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
       },
       "prevControl": {
-        "x": 3.57233606557377,
-        "y": 6.728227459016393
+        "x": 1.7333517849015447,
+        "y": 0.8254011541533226
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "HP"
     }
   ],
   "rotationTargets": [],
@@ -49,8 +49,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -58,13 +58,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -63.43494882292201
+    "rotation": -90.0
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -179.06080905426444
+    "rotation": -89.48708452392692
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BR-NW.path b/src/main/deploy/pathplanner/paths/BR-NW.path
new file mode 100644
index 00000000..1e7b60c9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BR-NW.path
@@ -0,0 +1,95 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.613086243763365,
+        "y": 1.3971240199572332
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2505345687811835,
+        "y": 0.7095260156806842
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.350691375623665,
+        "y": 0.609511760513185
+      },
+      "prevControl": {
+        "x": 3.350691375623665,
+        "y": 0.6095117605131851
+      },
+      "nextControl": {
+        "x": 5.350691375623665,
+        "y": 0.609511760513185
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.254444444444443,
+        "y": 0.9200505050505046
+      },
+      "prevControl": {
+        "x": 5.891898264931644,
+        "y": 0.5848663013499928
+      },
+      "nextControl": {
+        "x": 6.91703888492912,
+        "y": 1.5326378179514304
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.388481824661441,
+        "y": 6.2228118317890235
+      },
+      "prevControl": {
+        "x": 6.138481824661441,
+        "y": 6.2228118317890235
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.669473684210526,
+      "rotationDegrees": -179.96309845746404
+    },
+    {
+      "waypointRelativePos": 2.120000000000005,
+      "rotationDegrees": 90.31195699936117
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 88.12212255271466
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 179.70508837257486
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/C-CL.path b/src/main/deploy/pathplanner/paths/C-CL.path
new file mode 100644
index 00000000..cbf840e8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/C-CL.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5505773342836786,
+        "y": 3.997494654312188
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.250534568774933,
+        "y": 4.12251247327156
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.337904490371512,
+        "y": 4.9601318602993585
+      },
+      "prevControl": {
+        "x": 3.1005131860236856,
+        "y": 4.1850213827512475
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "CL"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -16.521034797124635
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 92.78713602149966
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/C-CR.path b/src/main/deploy/pathplanner/paths/C-CR.path
new file mode 100644
index 00000000..1b765f5a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/C-CR.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.574671717171718,
+        "y": 3.977739898989899
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.9690222222222222,
+        "y": 3.4308444444444435
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.2381888888888892,
+        "y": 2.8754111111111103
+      },
+      "prevControl": {
+        "x": 3.553688888888889,
+        "y": 3.9765333333333333
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 26.5650511770781
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/C-TWR.path b/src/main/deploy/pathplanner/paths/C-TWR.path
new file mode 100644
index 00000000..55909093
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/C-TWR.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5975757575757576,
+        "y": 4.035
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.622436769692649,
+        "y": 3.997494654312188
+      },
+      "isLocked": false,
+      "linkedName": "C"
+    },
+    {
+      "anchor": {
+        "x": 2.062865288667142,
+        "y": 4.035
+      },
+      "prevControl": {
+        "x": 3.0880114041339994,
+        "y": 3.997494654312188
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -1.2318158048334027
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/CR-DEP.path b/src/main/deploy/pathplanner/paths/CR-DEP.path
new file mode 100644
index 00000000..971ccb92
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/CR-DEP.path
@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.038,
+        "y": 2.81
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2881096445968256,
+        "y": 3.285199080049246
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.8376388888966055,
+        "y": 7.367537878793638
+      },
+      "prevControl": {
+        "x": 1.7661155797818637,
+        "y": 7.73892855514774
+      },
+      "nextControl": {
+        "x": 0.6055197161752905,
+        "y": 7.2746902097051125
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7231186868686872,
+        "y": 5.523762626262627
+      },
+      "prevControl": {
+        "x": 0.7917853535430703,
+        "y": 6.26785101010677
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DEPOT"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.4217543859649109,
+      "rotationDegrees": -115.20021424448595
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -114.56717132151371
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/CTLD-BLA.path b/src/main/deploy/pathplanner/paths/CTLD-BLA.path
new file mode 100644
index 00000000..e103a065
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/CTLD-BLA.path
@@ -0,0 +1,89 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.578282647584974,
+        "y": 4.464973166368515
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.604758497316638,
+        "y": 5.811681574239714
+      },
+      "isLocked": false,
+      "linkedName": "CTLD"
+    },
+    {
+      "anchor": {
+        "x": 6.1444722719141325,
+        "y": 5.649427549194991
+      },
+      "prevControl": {
+        "x": 7.539856887298749,
+        "y": 5.454722719141323
+      },
+      "nextControl": {
+        "x": 5.154067376548645,
+        "y": 5.787623581106455
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.6560107334525935,
+        "y": 5.649427549194991
+      },
+      "prevControl": {
+        "x": 3.1560107334525935,
+        "y": 5.649427549194991
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "BLA"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.0101694915254242,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -89.70002483769608
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/CTR-NWALL.path b/src/main/deploy/pathplanner/paths/CTR-NWALL.path
new file mode 100644
index 00000000..70cbd87c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/CTR-NWALL.path
@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.816500000000001,
+        "y": 4.1516142857142855
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.155685714285714,
+        "y": 4.281185714285714
+      },
+      "isLocked": false,
+      "linkedName": "QTRH"
+    },
+    {
+      "anchor": {
+        "x": 6.611485714285715,
+        "y": 4.786514285714286
+      },
+      "prevControl": {
+        "x": 6.747732801934569,
+        "y": 4.996125189789446
+      },
+      "nextControl": {
+        "x": 6.443042857142857,
+        "y": 4.5273714285714295
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.611485714285715,
+        "y": 2.1173428571428565
+      },
+      "prevControl": {
+        "x": 6.58557142857143,
+        "y": 2.661542857142856
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "CTR/NWALL"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7047244094488185,
+      "rotationDegrees": -89.80645139096313
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.3682983682983686,
+      "maxWaypointRelativePos": 1.934731934731934,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 0.07925407925407942,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -88.99491399474576
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 88.8367937547589
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/CTRD-BA.path b/src/main/deploy/pathplanner/paths/CTRD-BA.path
new file mode 100644
index 00000000..cd4b5e48
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/CTRD-BA.path
@@ -0,0 +1,89 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.480930232558139,
+        "y": 3.637477638640429
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.620983899821109,
+        "y": 2.3881216457960637
+      },
+      "isLocked": false,
+      "linkedName": "CTRD"
+    },
+    {
+      "anchor": {
+        "x": 5.901091234347048,
+        "y": 2.517924865831842
+      },
+      "prevControl": {
+        "x": 6.7448121645796055,
+        "y": 2.5016994633273693
+      },
+      "nextControl": {
+        "x": 4.901276094317208,
+        "y": 2.5371520800631857
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.493756708407871,
+        "y": 2.517924865831842
+      },
+      "prevControl": {
+        "x": 2.993756708407871,
+        "y": 2.517924865831842
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "BUMP-AFTER"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.0033898305084745,
+      "maxWaypointRelativePos": 1.667796610169492,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 88.36342295838341
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Copy of QTRL-MID.path b/src/main/deploy/pathplanner/paths/Copy of QTRL-MID.path
new file mode 100644
index 00000000..c3506a6d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Copy of QTRL-MID.path	
@@ -0,0 +1,121 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.95867469879518,
+        "y": 5.368180722891566
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.120451949980013,
+        "y": 7.485230485924739
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.854975903614458,
+        "y": 7.378879518072289
+      },
+      "prevControl": {
+        "x": 6.908561762200318,
+        "y": 7.333071437264206
+      },
+      "nextControl": {
+        "x": 4.855919745259401,
+        "y": 7.422316742348598
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.35787951807229,
+        "y": 7.411662650602409
+      },
+      "prevControl": {
+        "x": 4.851791865998495,
+        "y": 7.392758427352289
+      },
+      "nextControl": {
+        "x": 3.7510031405241824,
+        "y": 7.434890509906926
+      },
+      "isLocked": false,
+      "linkedName": "TLS"
+    },
+    {
+      "anchor": {
+        "x": 2.0853209700427953,
+        "y": 6.616262482168331
+      },
+      "prevControl": {
+        "x": 1.8385663965588042,
+        "y": 6.656414449302378
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0124333925399558,
+      "rotationDegrees": 179.65469616554114
+    },
+    {
+      "waypointRelativePos": 2.2,
+      "rotationDegrees": -178.76956835980303
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.386489479512739,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 2.2,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -138.66842599895932
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 2.0,
+    "rotation": -90.1883893275273
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Copy of TR-CTR-HALF.path b/src/main/deploy/pathplanner/paths/Copy of TR-CTR-HALF.path
new file mode 100644
index 00000000..50d3904c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Copy of TR-CTR-HALF.path	
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.7538673944809435,
+        "y": 0.702399999999999
+      },
+      "isLocked": false,
+      "linkedName": "trsj"
+    },
+    {
+      "anchor": {
+        "x": 5.48308888888889,
+        "y": 0.6364819277108438
+      },
+      "prevControl": {
+        "x": 4.456655922164885,
+        "y": 0.6091103819315393
+      },
+      "nextControl": {
+        "x": 6.213922222222224,
+        "y": 0.6559708165997311
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.313939759036145,
+        "y": 3.3684096385542173
+      },
+      "prevControl": {
+        "x": 6.391895314591702,
+        "y": 2.0236763052208846
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "nWallNew"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.9477178423236537,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.4622406639004049,
+      "rotationDegrees": 89.93611487106567
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.035437430786267494,
+      "maxWaypointRelativePos": 0.26578073089701393,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.3439205955334774,
+      "maxWaypointRelativePos": 1.891811414392056,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.39900744416872924,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/DEP-CL.path b/src/main/deploy/pathplanner/paths/DEP-CL.path
new file mode 100644
index 00000000..bb774fd8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/DEP-CL.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7231186868686872,
+        "y": 5.523762626262627
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.7689651818787465,
+        "y": 4.893427716159044
+      },
+      "isLocked": false,
+      "linkedName": "DEPOT"
+    },
+    {
+      "anchor": {
+        "x": 2.5439898989898997,
+        "y": 4.699217171717173
+      },
+      "prevControl": {
+        "x": 2.097361111111112,
+        "y": 4.974065656565657
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -114.56717132151371
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -114.56717132151371
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Disrupter2.path b/src/main/deploy/pathplanner/paths/Disrupter2.path
new file mode 100644
index 00000000..ec43fc6f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Disrupter2.path
@@ -0,0 +1,116 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.663626506024096,
+        "y": 3.8383012048192775
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.445072289156626,
+        "y": 3.051506024096385
+      },
+      "isLocked": false,
+      "linkedName": "Disrupt1End"
+    },
+    {
+      "anchor": {
+        "x": 6.445072289156626,
+        "y": 1.6090481927710845
+      },
+      "prevControl": {
+        "x": 6.484803196985326,
+        "y": 2.2617702499568617
+      },
+      "nextControl": {
+        "x": 6.368578313253011,
+        "y": 0.35236144578313167
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.0465542168674697,
+        "y": 0.6474096385542167
+      },
+      "prevControl": {
+        "x": 5.734771084337349,
+        "y": 0.669265060240964
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Disruptor2End"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 1.4058091286307017,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 1.8004962779156348,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherLow",
+      "waypointRelativePos": 1.3756823821339927,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherLow"
+        }
+      }
+    },
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 1.8719602977667555,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -178.9583733239901
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -117.69947280805494
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/HP-CR.path b/src/main/deploy/pathplanner/paths/HP-CR.path
new file mode 100644
index 00000000..6d3f2c50
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/HP-CR.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.3625291645117616,
+        "y": 0.8563997661853032
+      },
+      "isLocked": false,
+      "linkedName": "HP"
+    },
+    {
+      "anchor": {
+        "x": 2.4864194008559197,
+        "y": 1.45373751783167
+      },
+      "prevControl": {
+        "x": 1.8912410841654772,
+        "y": 1.1043937232524967
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 30.06858282186252
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/HP-DEP.path b/src/main/deploy/pathplanner/paths/HP-DEP.path
new file mode 100644
index 00000000..6eb67f3d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/HP-DEP.path
@@ -0,0 +1,105 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7127388036996465,
+        "y": 2.392228398442254
+      },
+      "isLocked": false,
+      "linkedName": "HP"
+    },
+    {
+      "anchor": {
+        "x": 1.925580808080809,
+        "y": 2.752373737373738
+      },
+      "prevControl": {
+        "x": 1.9141287878787883,
+        "y": 2.1454166666666676
+      },
+      "nextControl": {
+        "x": 1.9914810690227644,
+        "y": 6.245087567297212
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.8605429292929296,
+        "y": 7.3675378787878785
+      },
+      "prevControl": {
+        "x": 1.6524176020005827,
+        "y": 7.123884133339367
+      },
+      "nextControl": {
+        "x": 0.6215981771124424,
+        "y": 7.44105934099726
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7231186868686872,
+        "y": 5.523762626262627
+      },
+      "prevControl": {
+        "x": 0.6658585858585865,
+        "y": 7.046881313131314
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DEPOT"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 2.507368421052627,
+      "rotationDegrees": -113.28528920977637
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -114.56717132151371
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/HP-SHOOT.path b/src/main/deploy/pathplanner/paths/HP-SHOOT.path
new file mode 100644
index 00000000..13783827
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/HP-SHOOT.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.32337011683096,
+        "y": 1.2470263782402344
+      },
+      "isLocked": false,
+      "linkedName": "HP"
+    },
+    {
+      "anchor": {
+        "x": 1.719444444444445,
+        "y": 1.5728156565656572
+      },
+      "prevControl": {
+        "x": 1.0208712121212127,
+        "y": 1.13763888888889
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 31.865977693603632
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/NWALL-CLIMB.path b/src/main/deploy/pathplanner/paths/NWALL-CLIMB.path
new file mode 100644
index 00000000..3f7c2a55
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/NWALL-CLIMB.path
@@ -0,0 +1,168 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.8211,
+        "y": 5.240014285714285
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.507828571428572,
+        "y": 3.918385714285714
+      },
+      "isLocked": false,
+      "linkedName": "YHY"
+    },
+    {
+      "anchor": {
+        "x": 6.248685714285715,
+        "y": 1.7934142857142854
+      },
+      "prevControl": {
+        "x": 6.427571152485698,
+        "y": 2.7772841958141874
+      },
+      "nextControl": {
+        "x": 6.119114285714286,
+        "y": 1.080771428571428
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.587871428571429,
+        "y": 0.5495285714285705
+      },
+      "prevControl": {
+        "x": 6.087871428571429,
+        "y": 0.5495285714285705
+      },
+      "nextControl": {
+        "x": 5.087871428571429,
+        "y": 0.5495285714285706
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.8150428571428576,
+        "y": 0.5495285714285705
+      },
+      "prevControl": {
+        "x": 2.5650428571428576,
+        "y": 0.5495285714285705
+      },
+      "nextControl": {
+        "x": 3.0650428571428576,
+        "y": 0.5495285714285705
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.8714714285714285,
+        "y": 0.8475428571428563
+      },
+      "prevControl": {
+        "x": 1.2083571428571427,
+        "y": 0.6531857142857133
+      },
+      "nextControl": {
+        "x": 0.6549250320592783,
+        "y": 0.9724734705152506
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.6123285714285713,
+        "y": 2.3764857142857134
+      },
+      "prevControl": {
+        "x": 0.5864142857142856,
+        "y": 1.897071428571428
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.7618110236220474,
+      "rotationDegrees": 178.67153523769426
+    },
+    {
+      "waypointRelativePos": 3.484251968503936,
+      "rotationDegrees": -179.56362140522202
+    },
+    {
+      "waypointRelativePos": 4.18307086614173,
+      "rotationDegrees": 89.76480910221724
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 3.1002331002331003,
+      "maxWaypointRelativePos": 4.813519813519817,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherLow",
+      "waypointRelativePos": 0.9090909090909073,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherLow"
+        }
+      }
+    },
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 2.47086247086247,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 88.97696981133217
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 88.6677801461304
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/NWALL-DEP.path b/src/main/deploy/pathplanner/paths/NWALL-DEP.path
new file mode 100644
index 00000000..e532bf4f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/NWALL-DEP.path
@@ -0,0 +1,188 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.905781990521327,
+        "y": 1.3913744075829368
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.02540404040404,
+        "y": 3.496755050505052
+      },
+      "isLocked": false,
+      "linkedName": "HSE"
+    },
+    {
+      "anchor": {
+        "x": 6.277348484848485,
+        "y": 4.733573232323233
+      },
+      "prevControl": {
+        "x": 6.162828282828284,
+        "y": 3.8861237373737376
+      },
+      "nextControl": {
+        "x": 6.385039644017192,
+        "y": 5.530487810171672
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.071212121212121,
+        "y": 7.344905213270142
+      },
+      "prevControl": {
+        "x": 6.54405572310792,
+        "y": 7.151469194312795
+      },
+      "nextControl": {
+        "x": 5.747382754909533,
+        "y": 7.4773808631211836
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.9906186868686877,
+        "y": 7.344905213270142
+      },
+      "prevControl": {
+        "x": 3.3774907247834443,
+        "y": 7.32341232227463
+      },
+      "nextControl": {
+        "x": 2.013667352697342,
+        "y": 7.399180287391396
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.1021208530806337,
+        "y": 7.344905213270142
+      },
+      "prevControl": {
+        "x": 1.6609360189574758,
+        "y": 7.5275947867296065
+      },
+      "nextControl": {
+        "x": 0.6371110642226974,
+        "y": 7.192882782297581
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.8979383886256569,
+        "y": 5.3997985781988
+      },
+      "prevControl": {
+        "x": 0.6762921770843731,
+        "y": 5.5154400798727075
+      },
+      "nextControl": {
+        "x": 1.1451066350712193,
+        "y": 5.270841232226985
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.7254146919431275,
+        "y": 5.528755924170615
+      },
+      "prevControl": {
+        "x": 1.733651975156703,
+        "y": 5.278891666692161
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 2.065719360568384,
+      "rotationDegrees": 177.81982432474064
+    },
+    {
+      "waypointRelativePos": 2.838365896980463,
+      "rotationDegrees": -178.4194815244811
+    },
+    {
+      "waypointRelativePos": 4.152753108348143,
+      "rotationDegrees": -123.38150300596608
+    },
+    {
+      "waypointRelativePos": 4.872113676731794,
+      "rotationDegrees": -114.87019297935495
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 3.2292358803986843,
+      "maxWaypointRelativePos": 5.767441860465112,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.03986710963455847,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    },
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 2.631229235880409,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -25.887026266326675
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.56170533256645
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/NWALL-HP.path b/src/main/deploy/pathplanner/paths/NWALL-HP.path
new file mode 100644
index 00000000..8c6d578f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/NWALL-HP.path
@@ -0,0 +1,158 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.132575757575758,
+        "y": 7.069785353535353
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.033181818181819,
+        "y": 7.069785353535353
+      },
+      "isLocked": false,
+      "linkedName": "SEH"
+    },
+    {
+      "anchor": {
+        "x": 6.048308080808081,
+        "y": 7.069785353535353
+      },
+      "prevControl": {
+        "x": 6.323156565656564,
+        "y": 7.539318181818182
+      },
+      "nextControl": {
+        "x": 5.682961419895421,
+        "y": 6.445651474476225
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.899431818181819,
+        "y": 3.8288636363636366
+      },
+      "prevControl": {
+        "x": 5.8765277777777785,
+        "y": 4.561792929292929
+      },
+      "nextControl": {
+        "x": 5.911536972297631,
+        "y": 3.4414987046576666
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.899431818181819,
+        "y": 0.8003975903614469
+      },
+      "prevControl": {
+        "x": 6.250953816026736,
+        "y": 1.0039103259558735
+      },
+      "nextControl": {
+        "x": 5.464255050505051,
+        "y": 0.5484531459170012
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.505959595959596,
+        "y": 0.6795580808080808
+      },
+      "prevControl": {
+        "x": 3.7559595959595957,
+        "y": 0.6795580808080808
+      },
+      "nextControl": {
+        "x": 2.8646464646464653,
+        "y": 0.6795580808080809
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
+      },
+      "prevControl": {
+        "x": 1.490404040404041,
+        "y": 1.2636111111111108
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.1491228070175437,
+      "rotationDegrees": -91.30498426887868
+    },
+    {
+      "waypointRelativePos": 2.5614035087719005,
+      "rotationDegrees": -89.52336750739953
+    },
+    {
+      "waypointRelativePos": 3.1578947368420867,
+      "rotationDegrees": -179.710783645745
+    },
+    {
+      "waypointRelativePos": 3.9561403508771993,
+      "rotationDegrees": 179.31442234054273
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.25470653377631153,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    },
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 3.6877076411960146,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 89.60211903816543
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path
new file mode 100644
index 00000000..a12dfd68
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/New New Path.path	
@@ -0,0 +1,91 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.379734939759036,
+        "y": 0.6036987951807202
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.379734939759036,
+        "y": 0.60369879518072
+      },
+      "isLocked": false,
+      "linkedName": "TRS"
+    },
+    {
+      "anchor": {
+        "x": 7.754659090909092,
+        "y": 0.9315025252525266
+      },
+      "prevControl": {
+        "x": 6.774782542201074,
+        "y": 0.7318980431083035
+      },
+      "nextControl": {
+        "x": 8.373068181818182,
+        "y": 1.057474747474747
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.235643939393938,
+        "y": 4.01209595959596
+      },
+      "prevControl": {
+        "x": 8.304356060606061,
+        "y": 2.637853535353536
+      },
+      "nextControl": {
+        "x": 8.210675130921992,
+        "y": 4.511472129034879
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.132575757575758,
+        "y": 7.069785353535353
+      },
+      "prevControl": {
+        "x": 8.132575757575758,
+        "y": 6.073459595959596
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SEH"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.1000000000000099,
+      "rotationDegrees": 89.64085041980647
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 89.60211903816543
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.6213872948567607
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTR-BUMP.path b/src/main/deploy/pathplanner/paths/QTR-BUMP.path
new file mode 100644
index 00000000..fc920e6e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTR-BUMP.path
@@ -0,0 +1,89 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.592481927710843,
+        "y": 3.7399518072289153
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.598783542039356,
+        "y": 2.6477280858676204
+      },
+      "isLocked": false,
+      "linkedName": "QTRLong"
+    },
+    {
+      "anchor": {
+        "x": 5.641484794275492,
+        "y": 2.517924865831842
+      },
+      "prevControl": {
+        "x": 6.177249425116623,
+        "y": 2.544447867358631
+      },
+      "nextControl": {
+        "x": 4.002719141323793,
+        "y": 2.4367978533094794
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.493756708407871,
+        "y": 2.517924865831842
+      },
+      "prevControl": {
+        "x": 4.164973166368515,
+        "y": 2.5016994633273706
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "BUMP-AFTER"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.8203389830508477,
+      "maxWaypointRelativePos": 1.7627118644067767,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTR-TR.path b/src/main/deploy/pathplanner/paths/QTR-TR.path
new file mode 100644
index 00000000..269ad070
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTR-TR.path
@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.01866265060241,
+        "y": 3.586963855421687
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.938498509188269,
+        "y": 2.6708022392600714
+      },
+      "isLocked": false,
+      "linkedName": "CTR-QTR"
+    },
+    {
+      "anchor": {
+        "x": 7.122590361445783,
+        "y": 0.8550361445783141
+      },
+      "prevControl": {
+        "x": 7.66962167676118,
+        "y": 0.9714257861347814
+      },
+      "nextControl": {
+        "x": 6.046100462455883,
+        "y": 0.6259957405379102
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "prevControl": {
+        "x": 4.546022356091031,
+        "y": 0.6259060240963856
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "trsj"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.176763485477179,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.6877076411960215,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherLow",
+      "waypointRelativePos": 0.6372208436724653,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherLow"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTR-TRBack.path b/src/main/deploy/pathplanner/paths/QTR-TRBack.path
new file mode 100644
index 00000000..517ae9f6
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTR-TRBack.path
@@ -0,0 +1,112 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.98587951807229,
+        "y": 3.1717108433734937
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.905715376658149,
+        "y": 2.255549227211878
+      },
+      "isLocked": false,
+      "linkedName": "QTR"
+    },
+    {
+      "anchor": {
+        "x": 7.122590361445783,
+        "y": 0.8550361445783141
+      },
+      "prevControl": {
+        "x": 7.66962167676118,
+        "y": 0.9714257861347814
+      },
+      "nextControl": {
+        "x": 6.046100462455883,
+        "y": 0.6259957405379102
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.9919156626506025,
+        "y": 0.680192771084337
+      },
+      "prevControl": {
+        "x": 3.8770602409638535,
+        "y": 0.6036987951807236
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TRBack"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.176763485477179,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.3598014888337497,
+      "maxWaypointRelativePos": 1.689330024813893,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherLow",
+      "waypointRelativePos": 0.6372208436724653,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherLow"
+        }
+      }
+    },
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 1.6734491315136455,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTRH-HP.path b/src/main/deploy/pathplanner/paths/QTRH-HP.path
new file mode 100644
index 00000000..17f7e3d2
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTRH-HP.path
@@ -0,0 +1,105 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.215590361445784,
+        "y": 3.2809879518072296
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.303290361445786,
+        "y": 0.3576546184738967
+      },
+      "isLocked": false,
+      "linkedName": "nWall"
+    },
+    {
+      "anchor": {
+        "x": 4.662613636363636,
+        "y": 0.6108459595959597
+      },
+      "prevControl": {
+        "x": 6.925893890115048,
+        "y": 0.6362759383412605
+      },
+      "nextControl": {
+        "x": 2.4038444444444447,
+        "y": 0.5854666666666654
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
+      },
+      "prevControl": {
+        "x": 0.7765222222222223,
+        "y": 1.2968111111111114
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7087136929460608,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.2962655601659734,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.5049833887043119,
+      "maxWaypointRelativePos": 1.4440753045404362,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 1.046153846153844,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTRH-TRBack.path b/src/main/deploy/pathplanner/paths/QTRH-TRBack.path
new file mode 100644
index 00000000..eb37bdc9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTRH-TRBack.path
@@ -0,0 +1,105 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.313939759036145,
+        "y": 3.3684096385542173
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.401639759036146,
+        "y": 0.44507630522088437
+      },
+      "isLocked": false,
+      "linkedName": "nWallNew"
+    },
+    {
+      "anchor": {
+        "x": 4.662613636363636,
+        "y": 0.6108459595959597
+      },
+      "prevControl": {
+        "x": 6.9227336217661595,
+        "y": 0.4886091303499399
+      },
+      "nextControl": {
+        "x": 4.390662650602409,
+        "y": 0.6255542168674697
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.9919156626506025,
+        "y": 0.680192771084337
+      },
+      "prevControl": {
+        "x": 4.379734939759036,
+        "y": 0.669265060240964
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TRBack"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7087136929460608,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.2962655601659734,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.22431761786600546,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 1.046153846153844,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTRL-MID.path b/src/main/deploy/pathplanner/paths/QTRL-MID.path
new file mode 100644
index 00000000..23b09fea
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTRL-MID.path
@@ -0,0 +1,121 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.346722891566266,
+        "y": 4.668807228915662
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.390433734939759,
+        "y": 5.3463253012048195
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.346722891566266,
+        "y": 7.411662650602409
+      },
+      "prevControl": {
+        "x": 7.122590361445783,
+        "y": 7.3898072289156636
+      },
+      "nextControl": {
+        "x": 5.347119402287654,
+        "y": 7.439820495370818
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.35787951807229,
+        "y": 7.411662650602409
+      },
+      "prevControl": {
+        "x": 4.851791865998495,
+        "y": 7.392758427352289
+      },
+      "nextControl": {
+        "x": 3.7510031405241824,
+        "y": 7.434890509906926
+      },
+      "isLocked": false,
+      "linkedName": "TLS"
+    },
+    {
+      "anchor": {
+        "x": 2.4673855421686746,
+        "y": 7.16032530120482
+      },
+      "prevControl": {
+        "x": 2.2206309686846835,
+        "y": 7.200477268338867
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.49543568464731724,
+      "rotationDegrees": 1.0871828014246014
+    },
+    {
+      "waypointRelativePos": 2.2,
+      "rotationDegrees": 0.10378265392839682
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.386489479512739,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 2.2,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 18.824710018240136
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 2.0,
+    "rotation": -90.1883893275273
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTRL-TL.path b/src/main/deploy/pathplanner/paths/QTRL-TL.path
new file mode 100644
index 00000000..0c665ab8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTRL-TL.path
@@ -0,0 +1,161 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.346722891566266,
+        "y": 4.668807228915662
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.508500142751099,
+        "y": 6.785856991948837
+      },
+      "isLocked": false,
+      "linkedName": "CTR-QTRL"
+    },
+    {
+      "anchor": {
+        "x": 5.854975903614458,
+        "y": 7.378879518072289
+      },
+      "prevControl": {
+        "x": 6.908561762200318,
+        "y": 7.333071437264206
+      },
+      "nextControl": {
+        "x": 4.855919745259401,
+        "y": 7.422316742348598
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.35787951807229,
+        "y": 7.411662650602409
+      },
+      "prevControl": {
+        "x": 4.85215350797128,
+        "y": 7.411662650602409
+      },
+      "nextControl": {
+        "x": 3.8636055281732995,
+        "y": 7.411662650602409
+      },
+      "isLocked": false,
+      "linkedName": "TLS"
+    },
+    {
+      "anchor": {
+        "x": 0.9624170616113741,
+        "y": 7.495355450236968
+      },
+      "prevControl": {
+        "x": 1.1878249239316958,
+        "y": 7.623284596193915
+      },
+      "nextControl": {
+        "x": 0.6185308056872036,
+        "y": 7.300184462970367
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7367417061611372,
+        "y": 5.163376777251185
+      },
+      "prevControl": {
+        "x": 0.5096567616696486,
+        "y": 5.317794539505398
+      },
+      "nextControl": {
+        "x": 1.0054028436018954,
+        "y": 4.980687203791469
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.5958767772511853,
+        "y": 6.152049763033175
+      },
+      "prevControl": {
+        "x": 2.349122203767194,
+        "y": 6.1922017301672225
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0124333925399558,
+      "rotationDegrees": 179.65469616554114
+    },
+    {
+      "waypointRelativePos": 2.380106571936063,
+      "rotationDegrees": -178.76956835980303
+    },
+    {
+      "waypointRelativePos": 3.339253996447626,
+      "rotationDegrees": -113.45729127312542
+    },
+    {
+      "waypointRelativePos": 3.9076376554174104,
+      "rotationDegrees": -116.04952934713843
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.386489479512739,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 2.3622047244094206,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -48.44738685186516
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 2.0,
+    "rotation": -90.1883893275273
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTRLD-BLA.path b/src/main/deploy/pathplanner/paths/QTRLD-BLA.path
new file mode 100644
index 00000000..7a5540a5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTRLD-BLA.path
@@ -0,0 +1,89 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.614337349397591,
+        "y": 4.8982891566265065
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.4769492424079145,
+        "y": 5.29876336832123
+      },
+      "isLocked": false,
+      "linkedName": "QTRLH"
+    },
+    {
+      "anchor": {
+        "x": 6.1444722719141325,
+        "y": 5.649427549194991
+      },
+      "prevControl": {
+        "x": 6.598783542039356,
+        "y": 5.633202146690519
+      },
+      "nextControl": {
+        "x": 5.14510941756659,
+        "y": 5.685119079707403
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.6560107334525935,
+        "y": 5.649427549194991
+      },
+      "prevControl": {
+        "x": 3.1560107334525935,
+        "y": 5.649427549194991
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "BLA"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.0101694915254242,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -118.17859010995926
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTRLong-HP.path b/src/main/deploy/pathplanner/paths/QTRLong-HP.path
new file mode 100644
index 00000000..2730ad57
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTRLong-HP.path
@@ -0,0 +1,118 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.592481927710843,
+        "y": 3.7399518072289153
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.680181927710844,
+        "y": 0.8166184738955824
+      },
+      "isLocked": false,
+      "linkedName": "QTRLong"
+    },
+    {
+      "anchor": {
+        "x": 4.662613636363636,
+        "y": 0.6108459595959597
+      },
+      "prevControl": {
+        "x": 6.925893890115048,
+        "y": 0.6362759383412605
+      },
+      "nextControl": {
+        "x": 2.4038444444444447,
+        "y": 0.5854666666666654
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
+      },
+      "prevControl": {
+        "x": 0.7765222222222223,
+        "y": 1.2968111111111114
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7087136929460608,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.2962655601659734,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.5049833887043119,
+      "maxWaypointRelativePos": 1.137468982630273,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 4.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.129528535980151,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 2.0,
+        "maxAngularVelocity": 100.0,
+        "maxAngularAcceleration": 300.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 1.0660049627791606,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/QTRLong-TL.path b/src/main/deploy/pathplanner/paths/QTRLong-TL.path
new file mode 100644
index 00000000..16104dcf
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/QTRLong-TL.path
@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.614337349397591,
+        "y": 4.8982891566265065
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.158335446482184,
+        "y": 7.755920984382741
+      },
+      "isLocked": false,
+      "linkedName": "QTRLH"
+    },
+    {
+      "anchor": {
+        "x": 6.182807228915664,
+        "y": 7.324240963855422
+      },
+      "prevControl": {
+        "x": 6.707337349397591,
+        "y": 7.313313253012049
+      },
+      "nextControl": {
+        "x": 5.66708121700441,
+        "y": 7.334985255770239
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.7187228915662653,
+        "y": 7.324240963855422
+      },
+      "prevControl": {
+        "x": 7.3599702792211446,
+        "y": 7.408477559527209
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TL-Long"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7452282157676345,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.2,
+      "maxWaypointRelativePos": 0.3,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 1.8878411910669979,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -118.17859010995926
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/RightDisrupt1.path b/src/main/deploy/pathplanner/paths/RightDisrupt1.path
new file mode 100644
index 00000000..3344f0f4
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/RightDisrupt1.path
@@ -0,0 +1,109 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.314168674698795,
+        "y": 0.5709156626506025
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.559927710843374,
+        "y": 0.691120481927711
+      },
+      "isLocked": false,
+      "linkedName": "Disrupt1Start"
+    },
+    {
+      "anchor": {
+        "x": 8.401132530120481,
+        "y": 1.0845180722891563
+      },
+      "prevControl": {
+        "x": 8.207112700990953,
+        "y": 0.6277630578800578
+      },
+      "nextControl": {
+        "x": 8.92566265060241,
+        "y": 2.3193493975903605
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.401132530120481,
+        "y": 2.6253253012048186
+      },
+      "prevControl": {
+        "x": 8.38799282183979,
+        "y": 2.375670843871691
+      },
+      "nextControl": {
+        "x": 8.414272238401173,
+        "y": 2.874979758537946
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.663626506024096,
+        "y": 3.8383012048192775
+      },
+      "prevControl": {
+        "x": 7.670081902333116,
+        "y": 3.6790079515581304
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Disrupt1End"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.08347505865411542
+    },
+    {
+      "waypointRelativePos": 1.2464730290456414,
+      "rotationDegrees": 59.99999999999999
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "trench",
+      "minWaypointRelativePos": 0.06644518272425252,
+      "maxWaypointRelativePos": 0.3076923076923077,
+      "constraints": {
+        "maxVelocity": 6.0,
+        "maxAcceleration": 8.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -117.69947280805494
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 1.548157698977892
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TL-CTLD.path b/src/main/deploy/pathplanner/paths/TL-CTLD.path
new file mode 100644
index 00000000..6e7ad9ed
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TL-CTLD.path
@@ -0,0 +1,112 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.35787951807229,
+        "y": 7.411662650602409
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.35787951807229,
+        "y": 7.411662650602409
+      },
+      "isLocked": false,
+      "linkedName": "TLS"
+    },
+    {
+      "anchor": {
+        "x": 7.247799642218247,
+        "y": 7.239516994633274
+      },
+      "prevControl": {
+        "x": 6.248082968474112,
+        "y": 7.215714216686984
+      },
+      "nextControl": {
+        "x": 7.9292665474060815,
+        "y": 7.255742397137746
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.253774597495529,
+        "y": 7.1259391771019684
+      },
+      "prevControl": {
+        "x": 7.915766001965363,
+        "y": 7.333944466658994
+      },
+      "nextControl": {
+        "x": 8.466689176671665,
+        "y": 6.994914820685885
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.578282647584974,
+        "y": 4.464973166368515
+      },
+      "prevControl": {
+        "x": 8.643184257602861,
+        "y": 6.330894454382826
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "CTLD"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.7,
+      "rotationDegrees": -89.33092242292855
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.871186440677964,
+      "maxWaypointRelativePos": 3.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.3661016949152543,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -89.70002483769608
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TL-CTR-QTR.path b/src/main/deploy/pathplanner/paths/TL-CTR-QTR.path
new file mode 100644
index 00000000..11dba9af
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TL-CTR-QTR.path
@@ -0,0 +1,100 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.7187228915662653,
+        "y": 7.324240963855422
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7187228915662653,
+        "y": 7.324240963855422
+      },
+      "isLocked": false,
+      "linkedName": "TL-Long"
+    },
+    {
+      "anchor": {
+        "x": 6.204662650602408,
+        "y": 7.324240963855422
+      },
+      "prevControl": {
+        "x": 5.61694243289291,
+        "y": 7.411960399334451
+      },
+      "nextControl": {
+        "x": 6.936819277108433,
+        "y": 7.214963855421687
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.346722891566266,
+        "y": 4.668807228915662
+      },
+      "prevControl": {
+        "x": 6.324867469879518,
+        "y": 5.739722891566264
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "CTR-QTRL"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6190871369294697,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.4655601659750637,
+      "rotationDegrees": -85.29264819406744
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.41638981173864464,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.33945409429278844,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.1883893275273
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TL-CTR.path b/src/main/deploy/pathplanner/paths/TL-CTR.path
new file mode 100644
index 00000000..c5db5645
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TL-CTR.path
@@ -0,0 +1,86 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.445301204819277,
+        "y": 7.378879518072289
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.445301204819278,
+        "y": 7.378879518072289
+      },
+      "isLocked": false,
+      "linkedName": "TL"
+    },
+    {
+      "anchor": {
+        "x": 7.901197434069852,
+        "y": 7.104141414141413
+      },
+      "prevControl": {
+        "x": 7.751060379006985,
+        "y": 7.392525458486395
+      },
+      "nextControl": {
+        "x": 8.1554797979798,
+        "y": 6.615714513272424
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.901197434069852,
+        "y": 1.3971240199572332
+      },
+      "prevControl": {
+        "x": 7.838688524590165,
+        "y": 6.01028153955809
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.39438596491227657,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 0.8940350877192969,
+      "rotationDegrees": -90.47983053172646
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.36724565756823013,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -91.21887523513125
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -0.7046458978606126
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TL-DIS.path b/src/main/deploy/pathplanner/paths/TL-DIS.path
new file mode 100644
index 00000000..1bfcd517
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TL-DIS.path
@@ -0,0 +1,111 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.35787951807229,
+        "y": 7.411662650602409
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.35787951807229,
+        "y": 7.411662650602409
+      },
+      "isLocked": false,
+      "linkedName": "TLS"
+    },
+    {
+      "anchor": {
+        "x": 6.658033175355451,
+        "y": 7.355651658767772
+      },
+      "prevControl": {
+        "x": 5.658033175355451,
+        "y": 7.355651658767772
+      },
+      "nextControl": {
+        "x": 7.658033175355451,
+        "y": 7.355651658767772
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.065817535545024,
+        "y": 7.172962085308057
+      },
+      "prevControl": {
+        "x": 7.726873969729322,
+        "y": 7.547583921209622
+      },
+      "nextControl": {
+        "x": 8.474182464454977,
+        "y": 6.7216113744075825
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.22701421800948,
+        "y": 1.3913744075829368
+      },
+      "prevControl": {
+        "x": 8.495675355450237,
+        "y": 1.5740639810426522
+      },
+      "nextControl": {
+        "x": 7.768819494561197,
+        "y": 1.0798019956381042
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.905781990521327,
+        "y": 1.3913744075829368
+      },
+      "prevControl": {
+        "x": 5.60219490521327,
+        "y": 1.4048074644549753
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HSE"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 2.152753108348143,
+      "rotationDegrees": -89.92333279619605
+    },
+    {
+      "waypointRelativePos": 2.678507992895191,
+      "rotationDegrees": -89.64712703383984
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.56170533256645
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TL-Dep.path b/src/main/deploy/pathplanner/paths/TL-Dep.path
new file mode 100644
index 00000000..d2251556
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TL-Dep.path
@@ -0,0 +1,98 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.35787951807229,
+        "y": 7.411662650602409
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2978352393552344,
+        "y": 7.411662650602409
+      },
+      "isLocked": false,
+      "linkedName": "TLS"
+    },
+    {
+      "anchor": {
+        "x": 2.0859090909090914,
+        "y": 7.226694312796208
+      },
+      "prevControl": {
+        "x": 3.12251698647997,
+        "y": 6.968500234193897
+      },
+      "nextControl": {
+        "x": 0.9484337349397589,
+        "y": 7.51001204819277
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7002146464646473,
+        "y": 7.104141414141413
+      },
+      "prevControl": {
+        "x": 0.7835506113356967,
+        "y": 7.339842744131323
+      },
+      "nextControl": {
+        "x": 0.445759036144578,
+        "y": 6.384457831325301
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7231186868686872,
+        "y": 5.523762626262627
+      },
+      "prevControl": {
+        "x": 0.6772721918586281,
+        "y": 6.15409753636621
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DEPOT"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 2.073684210526315,
+      "rotationDegrees": -109.12578116282143
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.5210918114144006,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.2,
+    "rotation": -114.56717132151371
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TL-QTRH.path b/src/main/deploy/pathplanner/paths/TL-QTRH.path
new file mode 100644
index 00000000..68406848
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TL-QTRH.path
@@ -0,0 +1,105 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.35787951807229,
+        "y": 7.411662650602409
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.35787951807229,
+        "y": 7.411662650602409
+      },
+      "isLocked": false,
+      "linkedName": "TLS"
+    },
+    {
+      "anchor": {
+        "x": 6.947746987951808,
+        "y": 7.25867469879518
+      },
+      "prevControl": {
+        "x": 6.49665530561821,
+        "y": 7.429094936741865
+      },
+      "nextControl": {
+        "x": 7.548743489107949,
+        "y": 7.031621151271753
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.614337349397591,
+        "y": 4.8982891566265065
+      },
+      "prevControl": {
+        "x": 7.71268674698795,
+        "y": 6.373530120481928
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "QTRLH"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.28381742738588983,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.0970954356846392,
+      "rotationDegrees": -106.63717271459142
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.9151364764267971,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.43076923076921525,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -118.17859010995926
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TL-QTRHLong.path b/src/main/deploy/pathplanner/paths/TL-QTRHLong.path
new file mode 100644
index 00000000..5bb71104
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TL-QTRHLong.path
@@ -0,0 +1,118 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.35787951807229,
+        "y": 7.411662650602409
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.35787951807229,
+        "y": 7.411662650602409
+      },
+      "isLocked": false,
+      "linkedName": "TLS"
+    },
+    {
+      "anchor": {
+        "x": 6.947746987951808,
+        "y": 7.25867469879518
+      },
+      "prevControl": {
+        "x": 6.482007633724585,
+        "y": 7.38362915968541
+      },
+      "nextControl": {
+        "x": 7.395783132530121,
+        "y": 7.138469879518072
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.58155421686747,
+        "y": 4.1442771084337355
+      },
+      "prevControl": {
+        "x": 7.450421686746989,
+        "y": 5.608590361445783
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "QTRHLong"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.28381742738588983,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.0970954356846392,
+      "rotationDegrees": -51.64768397768135
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.9151364764267971,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.2803970223324992,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.43076923076921525,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -55.22216863363612
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-HALF.path b/src/main/deploy/pathplanner/paths/TR-CTR-HALF.path
new file mode 100644
index 00000000..21a3d7ba
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TR-CTR-HALF.path
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.7538673944809435,
+        "y": 0.702399999999999
+      },
+      "isLocked": false,
+      "linkedName": "trsj"
+    },
+    {
+      "anchor": {
+        "x": 5.48308888888889,
+        "y": 0.6364819277108438
+      },
+      "prevControl": {
+        "x": 4.456655922164885,
+        "y": 0.6091103819315393
+      },
+      "nextControl": {
+        "x": 6.213922222222224,
+        "y": 0.6559708165997311
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.215590361445784,
+        "y": 3.2809879518072296
+      },
+      "prevControl": {
+        "x": 6.226518072289158,
+        "y": 2.40677108433735
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "nWall"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.9477178423236537,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.4622406639004049,
+      "rotationDegrees": 89.93611487106567
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.035437430786267494,
+      "maxWaypointRelativePos": 0.26578073089701393,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.3439205955334774,
+      "maxWaypointRelativePos": 1.891811414392056,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.39900744416872924,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-HP.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-HP.path
new file mode 100644
index 00000000..c0201b44
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-HP.path
@@ -0,0 +1,220 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.66087777777778,
+        "y": 0.702399999999999
+      },
+      "isLocked": false,
+      "linkedName": "trsj"
+    },
+    {
+      "anchor": {
+        "x": 8.291855421686748,
+        "y": 1.0954457831325295
+      },
+      "prevControl": {
+        "x": 8.003767199322903,
+        "y": 0.6913739907260975
+      },
+      "nextControl": {
+        "x": 9.133289156626507,
+        "y": 2.2756385542168687
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.67455421686747,
+        "y": 3.4776867469879518
+      },
+      "prevControl": {
+        "x": 7.5652062262974065,
+        "y": 3.990486388780943
+      },
+      "nextControl": {
+        "x": 6.3139397590361455,
+        "y": 3.270060240963857
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.647349397590362,
+        "y": 2.6143975903614454
+      },
+      "prevControl": {
+        "x": 5.896485837151581,
+        "y": 2.5936362203980106
+      },
+      "nextControl": {
+        "x": 5.253951807228916,
+        "y": 2.647180722891566
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.07933734939759,
+        "y": 2.461409638554217
+      },
+      "prevControl": {
+        "x": 3.560156626506024,
+        "y": 2.505120481927711
+      },
+      "nextControl": {
+        "x": 2.6093551789393854,
+        "y": 2.4186839866943797
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3636867469879512,
+        "y": 1.7073975903614462
+      },
+      "prevControl": {
+        "x": 2.063060240963855,
+        "y": 2.406771084337348
+      },
+      "nextControl": {
+        "x": 0.6605244289301886,
+        "y": 1.0042352723036858
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
+      },
+      "prevControl": {
+        "x": 0.4847760133581298,
+        "y": 0.5551283829688684
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.2464730290456414,
+      "rotationDegrees": 111.59192207613846
+    },
+    {
+      "waypointRelativePos": 1.6741767764298092,
+      "rotationDegrees": -176.88494593076837
+    },
+    {
+      "waypointRelativePos": 2.897053726169844,
+      "rotationDegrees": -89.73393281353583
+    },
+    {
+      "waypointRelativePos": 4.1532062391681,
+      "rotationDegrees": -73.04060721630577
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "trench",
+      "minWaypointRelativePos": 0.06644518272425252,
+      "maxWaypointRelativePos": 0.47841191066996414,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "intake",
+      "minWaypointRelativePos": 1.2129032258064532,
+      "maxWaypointRelativePos": 1.9116625310173645,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "hopper",
+      "minWaypointRelativePos": 0.3831265508684809,
+      "maxWaypointRelativePos": 1.3439205955334927,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "moveShoot",
+      "minWaypointRelativePos": 3.722891566265057,
+      "maxWaypointRelativePos": 6.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 3.8000000000000034,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTR.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTR.path
new file mode 100644
index 00000000..0e336629
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTR.path
@@ -0,0 +1,131 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.66087777777778,
+        "y": 0.702399999999999
+      },
+      "isLocked": false,
+      "linkedName": "trsj"
+    },
+    {
+      "anchor": {
+        "x": 7.1444457831325305,
+        "y": 1.008024096385542
+      },
+      "prevControl": {
+        "x": 6.818077777777778,
+        "y": 0.6341888888888879
+      },
+      "nextControl": {
+        "x": 7.680087465265601,
+        "y": 1.6215698977468498
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.01866265060241,
+        "y": 3.586963855421687
+      },
+      "prevControl": {
+        "x": 8.005522942321718,
+        "y": 3.3373093980885598
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "CTR-QTR"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.2464730290456414,
+      "rotationDegrees": 89.06764595078421
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "trench",
+      "minWaypointRelativePos": 0.06644518272425252,
+      "maxWaypointRelativePos": 0.47841191066996414,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "intake",
+      "minWaypointRelativePos": 1.2129032258064532,
+      "maxWaypointRelativePos": 1.9116625310173645,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "hopper",
+      "minWaypointRelativePos": 0.3831265508684809,
+      "maxWaypointRelativePos": 1.3439205955334927,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.3366555924695484,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTRAngled.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTRAngled.path
new file mode 100644
index 00000000..8ab1c166
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTRAngled.path
@@ -0,0 +1,131 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.66087777777778,
+        "y": 0.702399999999999
+      },
+      "isLocked": false,
+      "linkedName": "trsj"
+    },
+    {
+      "anchor": {
+        "x": 7.461349397590361,
+        "y": 0.8987469879518073
+      },
+      "prevControl": {
+        "x": 7.134981392235608,
+        "y": 0.5249117804551532
+      },
+      "nextControl": {
+        "x": 7.996991079723431,
+        "y": 1.512292789313115
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.98587951807229,
+        "y": 3.1717108433734937
+      },
+      "prevControl": {
+        "x": 7.972739809791598,
+        "y": 2.9220563860403663
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "QTR"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.2464730290456414,
+      "rotationDegrees": 90.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "trench",
+      "minWaypointRelativePos": 0.06644518272425252,
+      "maxWaypointRelativePos": 0.47841191066996414,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "intake",
+      "minWaypointRelativePos": 1.0223325062034787,
+      "maxWaypointRelativePos": 1.9116625310173645,
+      "constraints": {
+        "maxVelocity": 2.5,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "hopper",
+      "minWaypointRelativePos": 0.3831265508684809,
+      "maxWaypointRelativePos": 0.9548387096774164,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.3366555924695484,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTRLong.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTRLong.path
new file mode 100644
index 00000000..54d3b75f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTRLong.path
@@ -0,0 +1,131 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.66087777777778,
+        "y": 0.702399999999999
+      },
+      "isLocked": false,
+      "linkedName": "trsj"
+    },
+    {
+      "anchor": {
+        "x": 7.1444457831325305,
+        "y": 1.008024096385542
+      },
+      "prevControl": {
+        "x": 6.818077777777778,
+        "y": 0.6341888888888879
+      },
+      "nextControl": {
+        "x": 7.680087465265601,
+        "y": 1.6215698977468498
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.592481927710843,
+        "y": 3.7399518072289153
+      },
+      "prevControl": {
+        "x": 7.579342219430152,
+        "y": 3.490297349895788
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "QTRLong"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.2464730290456414,
+      "rotationDegrees": 89.06764595078421
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "trench",
+      "minWaypointRelativePos": 0.06644518272425252,
+      "maxWaypointRelativePos": 0.47841191066996414,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "intake",
+      "minWaypointRelativePos": 1.2129032258064532,
+      "maxWaypointRelativePos": 1.9116625310173645,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "hopper",
+      "minWaypointRelativePos": 0.3831265508684809,
+      "maxWaypointRelativePos": 1.3439205955334927,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.3366555924695484,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TR-CTR.path b/src/main/deploy/pathplanner/paths/TR-CTR.path
new file mode 100644
index 00000000..298d6a37
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TR-CTR.path
@@ -0,0 +1,102 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.412518072289157,
+        "y": 0.6255542168674697
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.412518072289158,
+        "y": 0.6255542168674697
+      },
+      "isLocked": false,
+      "linkedName": "TR"
+    },
+    {
+      "anchor": {
+        "x": 7.614337349397591,
+        "y": 0.8768915662650603
+      },
+      "prevControl": {
+        "x": 6.97674647270479,
+        "y": 0.6393577102422535
+      },
+      "nextControl": {
+        "x": 8.107072430228255,
+        "y": 1.0604595375549144
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.756397590361445,
+        "y": 6.898060240963855
+      },
+      "prevControl": {
+        "x": 7.778253012048193,
+        "y": 4.909216867469879
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5291228070175418,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.090526315789475,
+      "rotationDegrees": 89.1329986792534
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.3942414174972331,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    },
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 1.0365448504983563,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 89.42127443439227
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -0.08211332881449039
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TR-CTRD.path b/src/main/deploy/pathplanner/paths/TR-CTRD.path
new file mode 100644
index 00000000..ba0f0757
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TR-CTRD.path
@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.379734939759036,
+        "y": 0.6036987951807202
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.3169767441860465,
+        "y": 0.7169051878354196
+      },
+      "isLocked": false,
+      "linkedName": "TRS"
+    },
+    {
+      "anchor": {
+        "x": 7.426279069767442,
+        "y": 0.702399999999999
+      },
+      "prevControl": {
+        "x": 6.426279069767442,
+        "y": 0.702399999999999
+      },
+      "nextControl": {
+        "x": 8.426279069767439,
+        "y": 0.702399999999999
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.480930232558139,
+        "y": 3.637477638640429
+      },
+      "prevControl": {
+        "x": 8.659409660107332,
+        "y": 1.5930769230769224
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "CTRD"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.272727272727273,
+      "rotationDegrees": 88.54844195970549
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.2542372881355925,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.37966101694915244,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 88.36342295838341
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.6213872948567607
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TR-HP.path b/src/main/deploy/pathplanner/paths/TR-HP.path
new file mode 100644
index 00000000..01b0029f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TR-HP.path
@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6608777777777783,
+        "y": 0.702399999999999
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.463825757575758,
+        "y": 1.2178030303030312
+      },
+      "isLocked": false,
+      "linkedName": "trsj"
+    },
+    {
+      "anchor": {
+        "x": 0.5331807228915662,
+        "y": 0.8003975903614469
+      },
+      "prevControl": {
+        "x": 1.9484848484848494,
+        "y": 1.18344696969697
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.20376522702104335,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TR-QTRH.path b/src/main/deploy/pathplanner/paths/TR-QTRH.path
new file mode 100644
index 00000000..8eb46e77
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TR-QTRH.path
@@ -0,0 +1,132 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.379734939759036,
+        "y": 0.6036987951807202
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.379734939759036,
+        "y": 0.60369879518072
+      },
+      "isLocked": false,
+      "linkedName": "TRS"
+    },
+    {
+      "anchor": {
+        "x": 6.598528571428571,
+        "y": 0.7697999999999992
+      },
+      "prevControl": {
+        "x": 5.598668320341805,
+        "y": 0.7530823956051358
+      },
+      "nextControl": {
+        "x": 7.233428571428571,
+        "y": 0.7804154905335634
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.816500000000001,
+        "y": 2.220999999999999
+      },
+      "prevControl": {
+        "x": 7.777628571428572,
+        "y": 1.521314285714285
+      },
+      "nextControl": {
+        "x": 7.844376993942099,
+        "y": 2.7227858909577494
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.816500000000001,
+        "y": 4.1516142857142855
+      },
+      "prevControl": {
+        "x": 7.816500000000001,
+        "y": 3.359531096798332
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "QTRH"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6673228346456703,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.4586614173228343,
+      "rotationDegrees": 90.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.6223776223776203,
+      "maxWaypointRelativePos": 3.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 9.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.3776223776223773,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "intakeIntake"
+        }
+      }
+    },
+    {
+      "name": "launcherPrep",
+      "waypointRelativePos": 0.7412587412587424,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "launcherPrep"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 88.8367937547589
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.6213872948567607
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TRBack-CTR-HALF.path b/src/main/deploy/pathplanner/paths/TRBack-CTR-HALF.path
new file mode 100644
index 00000000..f1cbe7a3
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TRBack-CTR-HALF.path
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.9919156626506025,
+        "y": 0.680192771084337
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.084905279353768,
+        "y": 0.680192771084337
+      },
+      "isLocked": false,
+      "linkedName": "TRBack"
+    },
+    {
+      "anchor": {
+        "x": 5.48308888888889,
+        "y": 0.6364819277108438
+      },
+      "prevControl": {
+        "x": 4.456655922164885,
+        "y": 0.6091103819315393
+      },
+      "nextControl": {
+        "x": 6.213922222222224,
+        "y": 0.6559708165997311
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.313939759036145,
+        "y": 3.3684096385542173
+      },
+      "prevControl": {
+        "x": 6.324867469879519,
+        "y": 2.4941927710843377
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "nWallNew"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.9477178423236537,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.4622406639004049,
+      "rotationDegrees": 89.93611487106567
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.035437430786267494,
+      "maxWaypointRelativePos": 0.26578073089701393,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.3439205955334774,
+      "maxWaypointRelativePos": 1.891811414392056,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "intakeIntake",
+      "waypointRelativePos": 0.39900744416872924,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 9.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 7642c343..0c65e4df 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -1,21 +1,28 @@
 {
-  "robotWidth": 0.8,
-  "robotLength": 0.8,
+  "robotWidth": 0.89535,
+  "robotLength": 0.89535,
   "holonomicMode": true,
   "pathFolders": [],
-  "autoFolders": [],
-  "defaultMaxVel": 5.25,
-  "defaultMaxAccel": 10.0,
+  "autoFolders": [
+    "AllianceSide",
+    "Center",
+    "Left",
+    "untested",
+    "Orbit",
+    "Right"
+  ],
+  "defaultMaxVel": 4.0,
+  "defaultMaxAccel": 9.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 40.0,
-  "robotMOI": 6.883,
+  "robotMass": 58.967,
+  "robotMOI": 8.0968,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.1016,
-  "driveGearing": 5.143,
+  "driveWheelRadius": 0.051,
+  "driveGearing": 6.0,
   "maxDriveSpeed": 5.45,
-  "driveMotorType": "NEO",
+  "driveMotorType": "krakenX60FOC",
   "driveCurrentLimit": 60.0,
   "wheelCOF": 1.2,
   "flModuleX": 0.273,
@@ -29,6 +36,8 @@
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": [
-    "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.3,\"y\":0.0},\"size\":{\"width\":0.2,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}"
+    "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.3,\"y\":0.0},\"size\":{\"width\":0.2,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}",
+    "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.55,\"y\":0.0},\"size\":{\"width\":0.65386,\"length\":0.3},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}",
+    "{\"name\":\"Robot Perimeter\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":0.0},\"size\":{\"width\":0.6858,\"length\":0.6858},\"borderRadius\":0.05,\"strokeWidth\":0.01,\"filled\":false}}"
   ]
 }
diff --git a/src/main/deploy/rebuilt_robot/Programming b/src/main/deploy/rebuilt_robot/Programming
new file mode 100644
index 00000000..5eff49a6
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/Programming
@@ -0,0 +1,2925 @@
+{
+  "version": 1.0,
+  "grid_size": 128,
+  "tabs": [
+    {
+      "name": "Teleoperated",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "HubStatus",
+            "x": 1024.0,
+            "y": 0.0,
+            "width": 512.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "AutoWinner",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/AutoWinner",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "CurrentShift",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/CurrentShift",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "MatchTime",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/MatchTime",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "NextShift",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/NextShift",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TimeRemainingInCurrentShift",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/TimeRemainingInCurrentShift",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "IsActiveNext",
+            "x": 896.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/HubStatus/IsActiveNext",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "ActiveNow",
+            "x": 896.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/HubStatus/ActiveNow",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Auto Modes",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "ComboBox Chooser",
+            "properties": {
+              "topic": "/SmartDashboard/Rebuilt/Auto Modes",
+              "period": 0.06,
+              "sort_options": false
+            }
+          },
+          {
+            "title": "HopperAngleDouble",
+            "x": 640.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/HopperAngleDouble",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "StateCurrent",
+            "x": 512.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/StateCurrent",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "StateRequested",
+            "x": 512.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/StateRequested",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Autonomous",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "Launcher",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 768.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "CoverageSatisfiesRange",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Boolean Box",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/CoverageSatisfiesRange",
+                  "period": 0.06,
+                  "data_type": "boolean",
+                  "true_color": 4283215696,
+                  "false_color": 4294198070,
+                  "true_icon": "None",
+                  "false_icon": "None"
+                }
+              },
+              {
+                "title": "FlyWheelMotorOutput",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelMotorOutput",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "FlyWheelSpeedActual",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelSpeedActual",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "FlyWheelSpeedCalculated",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelSpeedCalculated",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "FlyWheelSpeedDesired",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelSpeedDesired",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "FlyWheelSpeedError",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelSpeedError",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodAngleActual",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodAngleActual",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodAngleCalculated",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodAngleCalculated",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodAngleDesired",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodAngleDesired",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodAngleError",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodAngleError",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodVelocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodVelocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TargetDistance",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TargetDistance",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretAngleActual",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretAngleActual",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretAngleCalculated",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretAngleCalculated",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretAngleDesired",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretAngleDesired",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretAngleError",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretAngleError",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretVelocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretVelocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "UniqueCoverage",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/UniqueCoverage",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "translation",
+            "x": 1024.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "x",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Vision/Camera rear-prometheus/LatestTargetPose",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "struct_meta": {
+                    "path": [
+                      "translation",
+                      "x"
+                    ],
+                    "schema_name": "Pose3d",
+                    "type": "double"
+                  },
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "y",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Vision/Camera rear-prometheus/LatestTargetPose",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "struct_meta": {
+                    "path": [
+                      "translation",
+                      "y"
+                    ],
+                    "schema_name": "Pose3d",
+                    "type": "double"
+                  },
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "z",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Vision/Camera rear-prometheus/LatestTargetPose",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "struct_meta": {
+                    "path": [
+                      "translation",
+                      "z"
+                    ],
+                    "schema_name": "Pose3d",
+                    "type": "double"
+                  },
+                  "show_submit_button": false
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "Pose Field",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 768.0,
+            "height": 384.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/SmartDashboard/DrivePoseEstimator/Pose Field",
+              "period": 0.06,
+              "field_game": "Rebuilt",
+              "robot_width": 0.85,
+              "robot_length": 0.85,
+              "show_other_objects": true,
+              "show_trajectories": true,
+              "field_rotation": 0.0,
+              "robot_color": 4294198070,
+              "trajectory_color": 4294967295,
+              "show_robot_outside_widget": true
+            }
+          },
+          {
+            "title": "FlyWheelSpeedAtGoal",
+            "x": 1024.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/FlyWheelSpeedAtGoal",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "HoodAngleAtGoal",
+            "x": 1152.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/HoodAngleAtGoal",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "TurretAngleAtGoal",
+            "x": 1280.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/TurretAngleAtGoal",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "StateCurrent",
+            "x": 1024.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/StateCurrent",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "StateRequested",
+            "x": 1152.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/StateRequested",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "SimulatedGamepieces",
+            "x": 1280.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/SimulatedGamepieces",
+              "period": 0.06,
+              "data_type": "int",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "hasTarget",
+            "x": 512.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/photonvision/rear-prometheus/hasTarget",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "HasTarget",
+            "x": 640.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Vision/Camera rear-prometheus/HasTarget",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Connected",
+            "x": 640.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Vision/Camera rear-prometheus/Connected",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "left_Port_1184_Output_MJPEG_Server",
+            "x": 1280.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/left_Port_1184_Output_MJPEG_Server",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          },
+          {
+            "title": "Auto Modes",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "ComboBox Chooser",
+            "properties": {
+              "topic": "/SmartDashboard/Rebuilt/Auto Modes",
+              "period": 0.06,
+              "sort_options": false
+            }
+          },
+          {
+            "title": "Robot Pose3d",
+            "x": 128.0,
+            "y": 512.0,
+            "width": 512.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Shuffleboard/Pose/Robot Pose3d",
+              "period": 0.06,
+              "data_type": "double[]",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Hood",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "feedback",
+            "x": 512.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "kD",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/feedback/kD",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "kI",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/feedback/kI",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kP",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/feedback/kP",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          },
+          {
+            "title": "feedforward",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "kA",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/feedforward/kA",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kG",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/feedforward/kG",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kS",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/feedforward/kS",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kV",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/feedforward/kV",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "setpoint",
+            "x": 1024.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "position",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/setpoint/position",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "velocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/setpoint/velocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "motionprofile",
+            "x": 512.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "maxAcceleration",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/motionprofile/maxAcceleration",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "maxVelocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/closedloop/motionprofile/maxVelocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "hoodMotor",
+            "x": 768.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "Down",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Mechanisms/Commands/hoodMotor/Down",
+                  "period": 0.06,
+                  "show_type": true,
+                  "maximize_button_space": false
+                }
+              },
+              {
+                "title": "Up",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Mechanisms/Commands/hoodMotor/Up",
+                  "period": 0.06,
+                  "show_type": true,
+                  "maximize_button_space": false
+                }
+              },
+              {
+                "title": "ZeroEncoder",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Mechanisms/Commands/hoodMotor/ZeroEncoder",
+                  "period": 0.06,
+                  "show_type": true,
+                  "maximize_button_space": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "setpoint",
+            "x": 128.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "position",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hood/hoodMotor/closedloop/setpoint/position",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "velocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hood/hoodMotor/closedloop/setpoint/velocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "limit",
+            "x": 512.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "stator",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/current/limit/stator",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "supply",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hood/hoodMotor/current/limit/supply",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "current",
+            "x": 256.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "stator",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hood/hoodMotor/current/stator",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "supply",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hood/hoodMotor/current/supply",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "Live Tuning",
+            "x": 1280.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Mechanisms/Commands/Launcher/Live Tuning",
+              "period": 0.06,
+              "show_type": true,
+              "maximize_button_space": false
+            }
+          },
+          {
+            "title": "position",
+            "x": 1024.0,
+            "y": 256.0,
+            "width": 384.0,
+            "height": 384.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Mechanisms/hood/hoodMotor/mechanism/position",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 5.0,
+              "color": 4278238420,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "position",
+            "x": 896.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Mechanisms/hood/hoodMotor/closedloop/setpoint/position",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "HoodAngleAtGoal",
+            "x": 1536.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/HoodAngleAtGoal",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "HoodAngleActual",
+            "x": 1536.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/HoodAngleActual",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "HoodAngleCalculated",
+            "x": 1536.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/HoodAngleCalculated",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "HoodAngleError",
+            "x": 1536.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/HoodAngleError",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "HoodAngleDesired",
+            "x": 1792.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/HoodAngleDesired",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Turret",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "feedback",
+            "x": 512.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "kI",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/feedback/kI",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kP",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/feedback/kP",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "kD",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/feedback/kD",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          },
+          {
+            "title": "feedforward",
+            "x": 512.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "kA",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/feedforward/kA",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kG",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/feedforward/kG",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kS",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/feedforward/kS",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kV",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/feedforward/kV",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "setpoint",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "velocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/setpoint/velocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "position",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/setpoint/position",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          },
+          {
+            "title": "Launcher",
+            "x": 1280.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "Live Tuning",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Mechanisms/Commands/Launcher/Live Tuning",
+                  "period": 0.06,
+                  "show_type": true,
+                  "maximize_button_space": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "turretMotor",
+            "x": 768.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "Up",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Mechanisms/Commands/turretMotor/Up",
+                  "period": 0.06,
+                  "show_type": true,
+                  "maximize_button_space": false
+                }
+              },
+              {
+                "title": "ZeroEncoder",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Mechanisms/Commands/turretMotor/ZeroEncoder",
+                  "period": 0.06,
+                  "show_type": true,
+                  "maximize_button_space": false
+                }
+              },
+              {
+                "title": "Down",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Mechanisms/Commands/turretMotor/Down",
+                  "period": 0.06,
+                  "show_type": true,
+                  "maximize_button_space": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "current",
+            "x": 1024.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "stator",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hood/hoodMotor/current/stator",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "supply",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hood/hoodMotor/current/supply",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "limit",
+            "x": 1280.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "stator",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/current/limit/stator",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "supply",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/current/limit/supply",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "motionprofile",
+            "x": 1024.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "maxAcceleration",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/motionprofile/maxAcceleration",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "maxVelocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/turret/turretMotor/closedloop/motionprofile/maxVelocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "position",
+            "x": 384.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Mechanisms/turret/turretMotor/mechanism/position",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "position",
+            "x": 1408.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Mechanisms/turret/turretMotor/closedloop/setpoint/position",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "TurretAngleActual",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/TurretAngleActual",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "TurretAngleAtGoal",
+            "x": 256.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/TurretAngleAtGoal",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "CRT Angle",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/CRT Angle",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Coverage Satisfies Range",
+            "x": 768.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/SmartDashboard/Coverage Satisfies Range",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Unique Coverage",
+            "x": 896.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Unique Coverage",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Encoder 36",
+            "x": 1152.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Encoder 36",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Encoder 40",
+            "x": 1024.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Encoder 40",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "TurretAngleDesired",
+            "x": 0.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/TurretAngleDesired",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "TurretAngleError",
+            "x": 0.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/TurretAngleError",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "CRT Status",
+            "x": 384.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/CRT Status",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "StateCurrent",
+            "x": 384.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/StateCurrent",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Flywheel",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "feedback",
+            "x": 896.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "kD",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedback/kD",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "kI",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedback/kI",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kP",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedback/kP",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          },
+          {
+            "title": "feedforward",
+            "x": 1152.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "kA",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedforward/kA",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kG",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedforward/kG",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kS",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedforward/kS",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kV",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedforward/kV",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          },
+          {
+            "title": "setpoint",
+            "x": 1408.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "position",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/flywheel/flywheelMotor/closedloop/setpoint/position",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "velocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/flywheel/flywheelMotor/closedloop/setpoint/velocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "FlyWheelSpeedActual",
+            "x": 640.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/FlyWheelSpeedActual",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "FlyWheelSpeedAtGoal",
+            "x": 640.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/FlyWheelSpeedAtGoal",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "FlyWheelSpeedCalculated",
+            "x": 640.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/FlyWheelSpeedCalculated",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "FlyWheelSpeedDesired",
+            "x": 640.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/FlyWheelSpeedDesired",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "FlyWheelSpeedError",
+            "x": 640.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/FlyWheelSpeedError",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Live Tuning",
+            "x": 1152.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Mechanisms/Commands/Launcher/Live Tuning",
+              "period": 0.06,
+              "show_type": true,
+              "maximize_button_space": false
+            }
+          },
+          {
+            "title": "velocity",
+            "x": 1664.0,
+            "y": 128.0,
+            "width": 768.0,
+            "height": 640.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/Mechanisms/flywheel/flywheelMotor/mechanism/velocity",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 60.0,
+              "color": 4278238420,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "velocity",
+            "x": 1536.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Mechanisms/flywheel/flywheelMotor/rotor/velocity",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Hopper",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "feedback",
+            "x": 896.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "kD",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/closedloop/feedback/kD",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "kI",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/closedloop/feedback/kI",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kP",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/closedloop/feedback/kP",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          },
+          {
+            "title": "feedforward",
+            "x": 1152.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "kA",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/closedloop/feedforward/kA",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kG",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/closedloop/feedforward/kG",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kS",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/closedloop/feedforward/kS",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "kV",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/closedloop/feedforward/kV",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "setpoint",
+            "x": 128.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "velocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/closedloop/setpoint/velocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "position",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/closedloop/setpoint/position",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          },
+          {
+            "title": "limit",
+            "x": 1664.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "stator",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/current/limit/stator",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "supply",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Tuning/hopper/hopperMotor/current/limit/supply",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "current",
+            "x": 1920.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "stator",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hopper/hopperMotor/current/stator",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "supply",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hopper/hopperMotor/current/supply",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "setpoint",
+            "x": 1152.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "position",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hopper/hopperMotor/closedloop/setpoint/position",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "velocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Mechanisms/hopper/hopperMotor/closedloop/setpoint/velocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "HopperAngle",
+            "x": 384.0,
+            "y": 0.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/HopperAngle",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Speed",
+            "x": 640.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/Speed",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Live Tuning",
+            "x": 896.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Mechanisms/Commands/Intake/Live Tuning",
+              "period": 0.06,
+              "show_type": true,
+              "maximize_button_space": false
+            }
+          },
+          {
+            "title": "position",
+            "x": 640.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Mechanisms/hopper/hopperMotor/mechanism/position",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "position",
+            "x": 640.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/Mechanisms/hopper/hopperMotor/rotor/position",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Speed",
+            "x": 512.0,
+            "y": 512.0,
+            "width": 384.0,
+            "height": 384.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/Speed",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 30.0,
+              "color": 4278238420,
+              "line_width": 2.0
+            }
+          },
+          {
+            "title": "HopperAngleDouble",
+            "x": 384.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/HopperAngleDouble",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "StateCurrent",
+            "x": 512.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/StateCurrent",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "StateRequested",
+            "x": 512.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/StateRequested",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "StateCurrent",
+            "x": 896.0,
+            "y": 640.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Indexer/StateCurrent",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "StateRequested",
+            "x": 1024.0,
+            "y": 640.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Indexer/StateRequested",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "HopperAmps",
+            "x": 0.0,
+            "y": 256.0,
+            "width": 512.0,
+            "height": 384.0,
+            "type": "Graph",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/HopperAmps",
+              "period": 0.033,
+              "data_type": "double",
+              "time_displayed": 5.0,
+              "color": 4278238420,
+              "line_width": 2.0
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Shot Tune",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "ApplyGuess",
+            "x": 896.0,
+            "y": 0.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Toggle Button",
+            "properties": {
+              "topic": "/SmartDashboard/ShotTuning/ApplyGuess",
+              "period": 0.06,
+              "data_type": "boolean"
+            }
+          },
+          {
+            "title": "FireShot",
+            "x": 896.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/SmartDashboard/ShotTuning/FireShot",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "DistanceToTarget",
+            "x": 896.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/ShotTuning/DistanceToTarget",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "UseGuessSource",
+            "x": 896.0,
+            "y": 384.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/ShotTuning/UseGuessSource",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Tab 8",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Pose Field",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 1408.0,
+            "height": 768.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/SmartDashboard/DrivePoseEstimator/Pose Field",
+              "period": 0.06,
+              "field_game": "Rebuilt",
+              "robot_width": 0.85,
+              "robot_length": 0.85,
+              "show_other_objects": true,
+              "show_trajectories": true,
+              "field_rotation": 0.0,
+              "robot_color": 4294198070,
+              "trajectory_color": 4294967295,
+              "show_robot_outside_widget": true
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Tab 9",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "QuestNav",
+            "x": 1024.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 512.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "streams",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/QuestNav/streams",
+                  "period": 0.06,
+                  "data_type": "string[]",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "version",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/QuestNav/version",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "DistanceToVirtualTarget",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/DistanceToVirtualTarget",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "TargetDistance",
+            "x": 0.0,
+            "y": 128.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/TargetDistance",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Auto Modes",
+            "x": 384.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "ComboBox Chooser",
+            "properties": {
+              "topic": "/SmartDashboard/Rebuilt/Auto Modes",
+              "period": 0.06,
+              "sort_options": false
+            }
+          },
+          {
+            "title": "QUEST POSE",
+            "x": 512.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/QUEST POSE",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "QUEST POSE",
+            "x": 512.0,
+            "y": 384.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/QUEST POSE",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "QUEST Active",
+            "x": 512.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/SmartDashboard/QUEST Active",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "QUEST Connected",
+            "x": 640.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/SmartDashboard/QUEST Connected",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "QUEST POSE",
+            "x": 640.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/QUEST POSE",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Quest Calculated Offset to Robot Center",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/questnav/Quest Calculated Offset to Robot Center",
+              "period": 0.06,
+              "data_type": "double[]",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Quest POSE Update",
+            "x": 768.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/questnav/Quest POSE Update",
+              "period": 0.06,
+              "data_type": "double[]",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Flywheel Multiplier",
+            "x": 256.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Flywheel Multiplier",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Tab 10",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Turret Offset Angle",
+            "x": 640.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Turret Offset Angle",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "TurretAngleActual",
+            "x": 640.0,
+            "y": 256.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/TurretAngleActual",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/basic_robot/akit_swerve_drivetrain.json b/src/main/deploy/rebuilt_robot/akit_swerve_drivetrain.json
similarity index 81%
rename from src/main/deploy/basic_robot/akit_swerve_drivetrain.json
rename to src/main/deploy/rebuilt_robot/akit_swerve_drivetrain.json
index f1a74ad6..d05d193d 100644
--- a/src/main/deploy/basic_robot/akit_swerve_drivetrain.json
+++ b/src/main/deploy/rebuilt_robot/akit_swerve_drivetrain.json
@@ -10,50 +10,50 @@
       "uom": "inches"
     },
     "wheelDiameter": {
-      "val": 3.955,
-      "uom": "inches"
+      "val": 0.1002156588,
+      "uom": "meters"
     },
     "maxDriveSpeed": {
       "val": 4.69,
       "uom": "m/sec"
     },
     "bumperFrameLength": {
-      "val": 30,
+      "val": 33,
       "uom": "inches"
     },
     "bumperFrameWidth": {
-      "val": 30,
+      "val": 33,
       "uom": "inches"
     },
     "gyro": {
       "type": "pigeon2",
       "id": 50,
       "inverted": false,
-      "canbus": ""
+      "canbus": "canivore"
     },
-    "driveGearRatio": "1:6.75",
-    "steerGearRatio": "1:25",
+    "driveGearRatio": "1:6",
+    "steerGearRatio": "1:24",
     "driveMotorControl": {
       "feedBack": {
-        "p": 0.00001,
+        "p": 0.6,
         "i": 0.0,
         "d": 0.0
       },
       "feedForward": {
-        "s": 0.0698163127444402,
-        "v": 0.11987949085405275,
+        "s": 0.20406725776687384,
+        "v": 0.6682791477132728,
         "a": 0.0
       }
     },
     "steerMotorControl": {
       "feedBack": {
-        "p": 100,
+        "p": 30,
         "i": 0.0,
-        "d": 0.5
+        "d": 0.03
       },
       "feedForward": {
-        "s": 0.06242725612073021,
-        "v": 0.4513057312290134,
+        "s": 0.3245401421882499,
+        "v": 0.37097586102253277,
         "a": 0.0
       }
     },
@@ -63,17 +63,17 @@
           "name": "frontLeftDrive",
           "controllerType": "talonfx",
           "motorType": "KrakenX60",
-          "canId": 4
+          "canId": 1
         },
         "steerMotorSetup": {
           "name": "frontLeftSteer",
           "controllerType": "talonfx",
           "motorType": "Krakenx60",
-          "canId": 3
+          "canId": 2
         },
-        "encoderId": 13,
+        "encoderId": 1,
         "absoluteOffset": {
-          "val": 0.15234375,
+          "val": -0.11811640625,
           "uom": "rotations"
         },
         "encoderInverted": false
@@ -83,17 +83,17 @@
           "name": "frontRightDrive",
           "controllerType": "talonfx",
           "motorType": "Krakenx60",
-          "canId": 2
+          "canId": 7
         },
         "steerMotorSetup": {
           "name": "frontRightSteer",
           "controllerType": "talonfx",
           "motorType": "Krakenx60",
-          "canId": 1
+          "canId": 8
         },
-        "encoderId": 16,
+        "encoderId": 4,
         "absoluteOffset": {
-          "val": -0.4873046875,
+          "val": 0.055117578125,
           "uom": "rotations"
         },
         "encoderInverted": false
@@ -103,17 +103,17 @@
           "name": "backLeftDrive",
           "controllerType": "talonfx",
           "motorType": "Krakenx60",
-          "canId": 6
+          "canId": 3
         },
         "steerMotorSetup": {
           "name": "backLeftSteer",
           "controllerType": "talonfx",
           "motorType": "Krakenx60",
-          "canId": 5
+          "canId": 4
         },
-        "encoderId": 14,
+        "encoderId": 2,
         "absoluteOffset": {
-          "val": -0.219482421875,
+          "val": -0.094970703125,
           "uom": "rotations"
         },
         "encoderInverted": false
@@ -123,23 +123,23 @@
           "name": "backRightDrive",
           "controllerType": "talonfx",
           "motorType": "Krakenx60",
-          "canId": 8
+          "canId": 5
         },
         "steerMotorSetup": {
           "name": "backRightSteer",
           "controllerType": "talonfx",
           "motorType": "Krakenx60",
-          "canId": 7
+          "canId": 6
         },
-        "encoderId": 15,
+        "encoderId": 3,
         "absoluteOffset": {
-          "val": 0.17236328125,
+          "val": 0.24560546875,
           "uom": "rotations"
         },
         "encoderInverted": false
       }
     },
-    "coupleRatio": 3.8181818181818183,
+    "coupleRatio": 3.0,
     "invertLeftSide": true,
     "invertRightSide": true,
     "steerInertia": {
@@ -151,7 +151,7 @@
       "uom": "kg*m^2"
     },
     "robotMass": {
-      "val": 50,
+      "val": 68,
       "uom": "kg"
     },
     "wheelCOF": 1.2,
@@ -159,18 +159,18 @@
       "val": 120,
       "uom": "amps"
     },
-    "canbus": "",
+    "canbus": "canivore",
     "startingPose": {
       "x": {
-        "val": 3.42,
+        "val": 1.0,
         "uom": "m"
       },
       "y": {
-        "val": 5.75,
+        "val": 1.0,
         "uom": "m"
       },
       "rotation": {
-        "val": -45,
+        "val": 0.0,
         "uom": "deg"
       }
     }
diff --git a/src/main/deploy/basic_robot/cameras.json b/src/main/deploy/rebuilt_robot/cameras.json
similarity index 77%
rename from src/main/deploy/basic_robot/cameras.json
rename to src/main/deploy/rebuilt_robot/cameras.json
index ac73ba18..ba24ecd7 100644
--- a/src/main/deploy/basic_robot/cameras.json
+++ b/src/main/deploy/rebuilt_robot/cameras.json
@@ -1,8 +1,8 @@
 {
   "cameras": [
-    "left.json",
+    "rear.json",
     "right.json",
-    "quest.json"
+    "left.json"
   ],
   "viewGamePieces": false,
   "aprilTagLayout": "2026-rebuilt-andymark.json"
diff --git a/src/main/deploy/rebuilt_robot/cameras/left.json b/src/main/deploy/rebuilt_robot/cameras/left.json
new file mode 100644
index 00000000..15821926
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/cameras/left.json
@@ -0,0 +1,39 @@
+{
+  "name": "left-bagel",
+  "use": "apriltag",
+  "type": "photonvision",
+  "strategy": "LOWEST_AMBIGUITY",
+  "column": 0,
+  "x": -0.2778,
+  "y": 0.2975,
+  "z": 0.2368,
+  "roll": 0,
+  "pitch": -30,
+  "yaw": 70,
+  "targetFiducialIds": [
+    1,
+    2,
+    3,
+    4,
+    5,
+    6,
+    7,
+    8,
+    9,
+    10,
+    11,
+    12,
+    17,
+    18,
+    19,
+    20,
+    21,
+    22,
+    23,
+    24,
+    25,
+    26,
+    27,
+    28
+  ]
+}
diff --git a/src/main/deploy/basic_robot/cameras/quest.json b/src/main/deploy/rebuilt_robot/cameras/quest.json
similarity index 55%
rename from src/main/deploy/basic_robot/cameras/quest.json
rename to src/main/deploy/rebuilt_robot/cameras/quest.json
index df412311..21b84791 100644
--- a/src/main/deploy/basic_robot/cameras/quest.json
+++ b/src/main/deploy/rebuilt_robot/cameras/quest.json
@@ -2,10 +2,10 @@
   "name": "quest",
   "use": "quest",
   "column": 0,
-  "x": 0,
-  "y": 0,
-  "z": 18,
+  "x": -0.171968,
+  "y": -0.252501,
+  "z": 0.225336,
   "roll": 0,
   "pitch": 0,
-  "yaw": 0
+  "yaw": 180
 }
diff --git a/src/main/deploy/rebuilt_robot/cameras/rear.json b/src/main/deploy/rebuilt_robot/cameras/rear.json
new file mode 100644
index 00000000..658d4ac3
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/cameras/rear.json
@@ -0,0 +1,39 @@
+{
+  "name": "rear-prometheus",
+  "use": "apriltag",
+  "type": "photonvision",
+  "strategy": "LOWEST_AMBIGUITY",
+  "column": 0,
+  "x": -0.2961,
+  "y": -0.2212,
+  "z": 0.2368,
+  "roll": 0,
+  "pitch": -30,
+  "yaw": 175,
+  "targetFiducialIds": [
+    1,
+    2,
+    3,
+    4,
+    5,
+    6,
+    7,
+    8,
+    9,
+    10,
+    11,
+    12,
+    17,
+    18,
+    19,
+    20,
+    21,
+    22,
+    23,
+    24,
+    25,
+    26,
+    27,
+    28
+  ]
+}
diff --git a/src/main/deploy/rebuilt_robot/cameras/right.json b/src/main/deploy/rebuilt_robot/cameras/right.json
new file mode 100644
index 00000000..f748be7b
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/cameras/right.json
@@ -0,0 +1,39 @@
+{
+  "name": "right-raikou",
+  "use": "apriltag",
+  "type": "photonvision",
+  "strategy": "LOWEST_AMBIGUITY",
+  "column": 0,
+  "x": -0.2775,
+  "y": -0.3025,
+  "z": 0.2432,
+  "roll": 0,
+  "pitch": -30,
+  "yaw": -81,
+  "targetFiducialIds": [
+    1,
+    2,
+    3,
+    4,
+    5,
+    6,
+    7,
+    8,
+    9,
+    10,
+    11,
+    12,
+    17,
+    18,
+    19,
+    20,
+    21,
+    22,
+    23,
+    24,
+    25,
+    26,
+    27,
+    28
+  ]
+}
diff --git a/src/main/deploy/baby_swerve/competition_mode.json b/src/main/deploy/rebuilt_robot/competition_mode.json
similarity index 100%
rename from src/main/deploy/baby_swerve/competition_mode.json
rename to src/main/deploy/rebuilt_robot/competition_mode.json
diff --git a/src/main/deploy/basic_robot/controllers.json b/src/main/deploy/rebuilt_robot/controllers.json
similarity index 100%
rename from src/main/deploy/basic_robot/controllers.json
rename to src/main/deploy/rebuilt_robot/controllers.json
diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_left_trigger.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_left_trigger.json
similarity index 100%
rename from src/main/deploy/baby_swerve/controllers/axis/driver_left_trigger.json
rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_left_trigger.json
diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_left_x.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_left_x.json
similarity index 100%
rename from src/main/deploy/baby_swerve/controllers/axis/driver_left_x.json
rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_left_x.json
diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_left_y.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_left_y.json
similarity index 100%
rename from src/main/deploy/baby_swerve/controllers/axis/driver_left_y.json
rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_left_y.json
diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_right_trigger.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_right_trigger.json
similarity index 100%
rename from src/main/deploy/baby_swerve/controllers/axis/driver_right_trigger.json
rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_right_trigger.json
diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_right_x.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_right_x.json
similarity index 100%
rename from src/main/deploy/baby_swerve/controllers/axis/driver_right_x.json
rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_right_x.json
diff --git a/src/main/deploy/baby_swerve/controllers/axis/operator_left_y.json b/src/main/deploy/rebuilt_robot/controllers/axis/operator_left_y.json
similarity index 100%
rename from src/main/deploy/baby_swerve/controllers/axis/operator_left_y.json
rename to src/main/deploy/rebuilt_robot/controllers/axis/operator_left_y.json
diff --git a/src/main/deploy/baby_swerve/controllers/axis/operator_right_y.json b/src/main/deploy/rebuilt_robot/controllers/axis/operator_right_y.json
similarity index 100%
rename from src/main/deploy/baby_swerve/controllers/axis/operator_right_y.json
rename to src/main/deploy/rebuilt_robot/controllers/axis/operator_right_y.json
diff --git a/src/main/deploy/baby_swerve/controllers/driver.json b/src/main/deploy/rebuilt_robot/controllers/driver.json
similarity index 100%
rename from src/main/deploy/baby_swerve/controllers/driver.json
rename to src/main/deploy/rebuilt_robot/controllers/driver.json
diff --git a/src/main/deploy/baby_swerve/controllers/operator.json b/src/main/deploy/rebuilt_robot/controllers/operator.json
similarity index 100%
rename from src/main/deploy/baby_swerve/controllers/operator.json
rename to src/main/deploy/rebuilt_robot/controllers/operator.json
diff --git a/src/main/deploy/basic_robot/demo_mode.json b/src/main/deploy/rebuilt_robot/demo_mode.json
similarity index 100%
rename from src/main/deploy/basic_robot/demo_mode.json
rename to src/main/deploy/rebuilt_robot/demo_mode.json
diff --git a/src/main/deploy/baby_swerve/drive_modules/backleft.json b/src/main/deploy/rebuilt_robot/drive_modules/backleft.json
similarity index 100%
rename from src/main/deploy/baby_swerve/drive_modules/backleft.json
rename to src/main/deploy/rebuilt_robot/drive_modules/backleft.json
diff --git a/src/main/deploy/baby_swerve/drive_modules/backright.json b/src/main/deploy/rebuilt_robot/drive_modules/backright.json
similarity index 100%
rename from src/main/deploy/baby_swerve/drive_modules/backright.json
rename to src/main/deploy/rebuilt_robot/drive_modules/backright.json
diff --git a/src/main/deploy/baby_swerve/drive_modules/frontleft.json b/src/main/deploy/rebuilt_robot/drive_modules/frontleft.json
similarity index 100%
rename from src/main/deploy/baby_swerve/drive_modules/frontleft.json
rename to src/main/deploy/rebuilt_robot/drive_modules/frontleft.json
diff --git a/src/main/deploy/baby_swerve/drive_modules/frontright.json b/src/main/deploy/rebuilt_robot/drive_modules/frontright.json
similarity index 100%
rename from src/main/deploy/baby_swerve/drive_modules/frontright.json
rename to src/main/deploy/rebuilt_robot/drive_modules/frontright.json
diff --git a/src/main/deploy/rebuilt_robot/elastic-layout.json b/src/main/deploy/rebuilt_robot/elastic-layout.json
new file mode 100644
index 00000000..1c6e1d4f
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/elastic-layout.json
@@ -0,0 +1,701 @@
+{
+  "version": 1.0,
+  "grid_size": 128,
+  "tabs": [
+    {
+      "name": "Teleoperated",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "HubStatus",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 512.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "AutoWinner",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/AutoWinner",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "CurrentShift",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/CurrentShift",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "MatchTime",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/MatchTime",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "NextShift",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/NextShift",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TimeRemainingInCurrentShift",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/HubStatus/TimeRemainingInCurrentShift",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "IsActiveNext",
+            "x": 640.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/HubStatus/IsActiveNext",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "ActiveNow",
+            "x": 640.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/HubStatus/ActiveNow",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Autonomous",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "Indexer",
+            "x": 1280.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "SpindexerSpeed",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Indexer/SpindexerSpeed",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "StateCurrent",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Indexer/StateCurrent",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "StateRequested",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Indexer/StateRequested",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TransferBackSpeed",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Indexer/TransferBackSpeed",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TransferFrontSpeed",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Indexer/TransferFrontSpeed",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "Intake",
+            "x": 1024.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "HopperAngle",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Intake/HopperAngle",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "Speed",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Intake/Speed",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "StateCurrent",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Intake/StateCurrent",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "StateRequested",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Intake/StateRequested",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "Launcher",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 768.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "CoverageSatisfiesRange",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Boolean Box",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/CoverageSatisfiesRange",
+                  "period": 0.06,
+                  "data_type": "boolean",
+                  "true_color": 4283215696,
+                  "false_color": 4294198070,
+                  "true_icon": "None",
+                  "false_icon": "None"
+                }
+              },
+              {
+                "title": "FlyWheelMotorOutput",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelMotorOutput",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "FlyWheelSpeedActual",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelSpeedActual",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "FlyWheelSpeedCalculated",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelSpeedCalculated",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "FlyWheelSpeedDesired",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelSpeedDesired",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "FlyWheelSpeedError",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/FlyWheelSpeedError",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodAngleActual",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodAngleActual",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodAngleCalculated",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodAngleCalculated",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodAngleDesired",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodAngleDesired",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodAngleError",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodAngleError",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "HoodVelocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/HoodVelocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TargetDistance",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TargetDistance",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretAngleActual",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretAngleActual",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretAngleCalculated",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretAngleCalculated",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretAngleDesired",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretAngleDesired",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretAngleError",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretAngleError",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "TurretVelocity",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/TurretVelocity",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "UniqueCoverage",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/AdvantageKit/Launcher/UniqueCoverage",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "left-processed",
+            "x": 384.0,
+            "y": 384.0,
+            "width": 384.0,
+            "height": 384.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/left-processed",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          },
+          {
+            "title": "right-processed",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 384.0,
+            "height": 384.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/right-processed",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          },
+          {
+            "title": "Pose Field",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 768.0,
+            "height": 384.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/SmartDashboard/DrivePoseEstimator/Pose Field",
+              "period": 0.06,
+              "field_game": "Rebuilt",
+              "robot_width": 0.85,
+              "robot_length": 0.85,
+              "show_other_objects": true,
+              "show_trajectories": true,
+              "field_rotation": 0.0,
+              "robot_color": 4294198070,
+              "trajectory_color": 4294967295,
+              "show_robot_outside_widget": true
+            }
+          },
+          {
+            "title": "FlyWheelSpeedAtGoal",
+            "x": 1024.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/FlyWheelSpeedAtGoal",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "HoodAngleAtGoal",
+            "x": 1152.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/HoodAngleAtGoal",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "TurretAngleAtGoal",
+            "x": 1280.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/TurretAngleAtGoal",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "StateCurrent",
+            "x": 1024.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/StateCurrent",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "StateRequested",
+            "x": 1152.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Launcher/StateRequested",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "SimulatedGamepieces",
+            "x": 1280.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Intake/SimulatedGamepieces",
+              "period": 0.06,
+              "data_type": "int",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    }
+  ]
+}
diff --git a/src/main/deploy/basic_robot/field/game_pieces.json b/src/main/deploy/rebuilt_robot/field/game_pieces.json
similarity index 78%
rename from src/main/deploy/basic_robot/field/game_pieces.json
rename to src/main/deploy/rebuilt_robot/field/game_pieces.json
index a1e2a1fb..b13b37c1 100644
--- a/src/main/deploy/basic_robot/field/game_pieces.json
+++ b/src/main/deploy/rebuilt_robot/field/game_pieces.json
@@ -3,7 +3,7 @@
     {
       "x": 2,
       "y": 7,
-      "type": "Fuel",
+      "type": "Coral",
       "rotation": 0
     }
   ]
diff --git a/src/main/deploy/basic_robot/robot.json b/src/main/deploy/rebuilt_robot/robot.json
similarity index 80%
rename from src/main/deploy/basic_robot/robot.json
rename to src/main/deploy/rebuilt_robot/robot.json
index 2475d0f1..9491e148 100644
--- a/src/main/deploy/basic_robot/robot.json
+++ b/src/main/deploy/rebuilt_robot/robot.json
@@ -1,11 +1,11 @@
 {
   "userConfig": "competition_mode.json",
   "driveType": "AKIT_SWERVE_DRIVE",
-  "trackWidth": 16.8,
+  "trackWidth": 22,
   "trackWidthUom": "in",
-  "wheelBase": 18.5,
+  "wheelBase": 22,
   "wheelBaseUom": "in",
-  "wheelDiameter": 0.103386904,
+  "wheelDiameter": 0.1002156588,
   "wheelDiameterUom": "m",
   "physicalMaxSpeed": 5.93,
   "physicalMaxSpeedUom": "m/s",
diff --git a/src/main/deploy/rebuilt_robot/subsystems/climb.json b/src/main/deploy/rebuilt_robot/subsystems/climb.json
new file mode 100644
index 00000000..3dda928d
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/climb.json
@@ -0,0 +1,8 @@
+{
+  "devices": [
+    {
+      "device": "yams_elevator",
+      "file": "climb/lifter.json"
+    }
+  ]
+}
diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_elevator.json b/src/main/deploy/rebuilt_robot/subsystems/climb/lifter.json
similarity index 78%
rename from src/main/deploy/basic_robot/subsystems/example/yams_elevator.json
rename to src/main/deploy/rebuilt_robot/subsystems/climb/lifter.json
index e8cbe550..ef3812e2 100644
--- a/src/main/deploy/basic_robot/subsystems/example/yams_elevator.json
+++ b/src/main/deploy/rebuilt_robot/subsystems/climb/lifter.json
@@ -1,9 +1,10 @@
 {
   "motorSetup": {
-    "name": "Elevator",
-    "controllerType": "spark",
-    "motorType": "Neo",
-    "canId": 10,
+    "name": "lifter",
+    "controllerType": "talonfx",
+    "motorType": "KrakenX60",
+    "canId": 20,
+    "logLevel": "LOW",
     "robotToMotor": {
       "x": {
         "val": -10.7,
@@ -23,6 +24,7 @@
       }
     }
   },
+  "controlAlgorithm": "SIMPLE",
   "motorSystemId": {
     "feedBack": {
       "p": 2.7739E-10,
@@ -64,35 +66,36 @@
     "maxAcceleration": {
       "val": 20,
       "uom": "m/s^2"
-    }
+    },
+    "controlMode": "CLOSED_LOOP"
   },
   "sprocketTeeth": 22,
   "lowerSoftLimit": {
-    "val": 0.1,
-    "uom": "m"
+    "val": 0,
+    "uom": "in"
   },
   "upperSoftLimit": {
-    "val": 2,
-    "uom": "m"
+    "val": 9,
+    "uom": "in"
   },
   "lowerHardLimit": {
-    "val": 0.1,
-    "uom": "m"
+    "val": 0,
+    "uom": "in"
   },
   "upperHardLimit": {
-    "val": 3,
-    "uom": "m"
+    "val": 26.5,
+    "uom": "in"
   },
   "gearing": [
     3,
     4
   ],
   "startingPosition": {
-    "val": 0.5,
+    "val": 0,
     "uom": "m"
   },
   "mass": {
-    "val": 16,
+    "val": 5,
     "uom": "lbs"
   }
 }
diff --git a/src/main/deploy/rebuilt_robot/subsystems/indexer.json b/src/main/deploy/rebuilt_robot/subsystems/indexer.json
new file mode 100644
index 00000000..58cfb171
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/indexer.json
@@ -0,0 +1,12 @@
+{
+  "devices": [
+    {
+      "device": "yams_shooter",
+      "file": "indexer/transfer_flywheel.json"
+    },
+    {
+      "device": "percent_motor",
+      "file": "indexer/spindexer.json"
+    }
+  ]
+}
diff --git a/src/main/deploy/basic_robot/subsystems/example/percent_motor.json b/src/main/deploy/rebuilt_robot/subsystems/indexer/spindexer.json
similarity index 52%
rename from src/main/deploy/basic_robot/subsystems/example/percent_motor.json
rename to src/main/deploy/rebuilt_robot/subsystems/indexer/spindexer.json
index 93be5418..bacd343e 100644
--- a/src/main/deploy/basic_robot/subsystems/example/percent_motor.json
+++ b/src/main/deploy/rebuilt_robot/subsystems/indexer/spindexer.json
@@ -1,8 +1,8 @@
 {
-  "name": "percent_motor",
-  "controller": "spark",
-  "type": "Neo",
-  "id": 11,
+  "name": "spindexer",
+  "controller": "talonfx",
+  "type": "KrakenX44",
+  "id": 9,
   "gearing": 1.0,
   "momentOfInertiaKgMSq": 1.0,
   "x": 0.5,
diff --git a/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_back.json b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_back.json
new file mode 100644
index 00000000..75775998
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_back.json
@@ -0,0 +1,10 @@
+{
+  "name": "transfer_back",
+  "controller": "talonfx",
+  "type": "KrakenX60",
+  "id": 11,
+  "gearing": 1.0,
+  "x": -0.236,
+  "y": -0.001,
+  "z": 0.136
+}
diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_pivot.json b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_flywheel.json
similarity index 73%
rename from src/main/deploy/basic_robot/subsystems/example/yams_pivot.json
rename to src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_flywheel.json
index d3d3b2ba..83ea7c16 100644
--- a/src/main/deploy/basic_robot/subsystems/example/yams_pivot.json
+++ b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_flywheel.json
@@ -1,9 +1,17 @@
 {
   "motorSetup": {
-    "name": "Turret",
+    "name": "transfer",
     "controllerType": "talonfx",
     "motorType": "KrakenX60",
-    "canId": 15,
+    "canId": 10,
+    "logLevel": "LOW",
+    "inverted": true,
+    "followers": [
+      {
+        "canId": 11,
+        "inverted": false
+      }
+    ],
     "robotToMotor": {
       "x": {
         "val": -5.872,
@@ -21,8 +29,7 @@
         "val": 0.0,
         "uom": "deg"
       }
-    },
-    "movementPlane": "XY"
+    }
   },
   "motorSystemId": {
     "feedBack": {
@@ -64,33 +71,21 @@
       "uom": "deg/s^2"
     }
   },
-  "lowerHardLimit": {
-    "val": -165,
-    "uom": "deg"
-  },
-  "upperHardLimit": {
-    "val": 165,
-    "uom": "deg"
-  },
-  "startingAngle": {
-    "val": 0,
-    "uom": "deg"
-  },
   "lowerSoftLimit": {
-    "val": -160,
-    "uom": "deg"
+    "val": 0,
+    "uom": "rpm"
   },
   "upperSoftLimit": {
-    "val": 160,
-    "uom": "deg"
-  },
-  "gearStages": "12:36:40:12:108",
-  "radius": {
-    "val": 5,
-    "uom": "in"
+    "val": 5000,
+    "uom": "rpm"
   },
+  "gearStages": "1:1",
   "mass": {
-    "val": 20,
-    "uom": "lb"
+    "val": 2,
+    "uom": "kg"
+  },
+  "radius": {
+    "val": 0.05,
+    "uom": "m"
   }
 }
diff --git a/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_front.json b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_front.json
new file mode 100644
index 00000000..c76c9866
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_front.json
@@ -0,0 +1,10 @@
+{
+  "name": "transfer_front",
+  "controller": "talonfx",
+  "type": "KrakenX60",
+  "id": 10,
+  "gearing": 1.0,
+  "x": -0.236,
+  "y": -0.001,
+  "z": 0.136
+}
diff --git a/src/main/deploy/rebuilt_robot/subsystems/intake.json b/src/main/deploy/rebuilt_robot/subsystems/intake.json
new file mode 100644
index 00000000..342b728d
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/intake.json
@@ -0,0 +1,16 @@
+{
+  "devices": [
+    {
+      "device": "yams_shooter",
+      "file": "intake/spintake_inner.json"
+    },
+    {
+      "device": "yams_shooter",
+      "file": "intake/spintake_outer.json"
+    },
+    {
+      "device": "yams_arm",
+      "file": "intake/hopper.json"
+    }
+  ]
+}
diff --git a/src/main/deploy/rebuilt_robot/subsystems/intake/hopper.json b/src/main/deploy/rebuilt_robot/subsystems/intake/hopper.json
new file mode 100644
index 00000000..f768d1da
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/intake/hopper.json
@@ -0,0 +1,96 @@
+{
+  "motorSetup": {
+    "name": "hopper",
+    "controllerType": "talonfx",
+    "motorType": "KrakenX44",
+    "canId": 15,
+    "logLevel": "LOW",
+    "inverted": true,
+    "currentLimit": {
+      "val": 60,
+      "uom": "amps"
+    },
+    "followers": [
+      {
+        "canId": 14,
+        "inverted": true
+      }
+    ]
+  },
+  "controlAlgorithm": "PROFILED",
+  "motorSystemId": {
+    "feedBack": {
+      "p": 100,
+      "i": 0.0,
+      "d": 0.0
+    },
+    "feedForward": {
+      "s": 0.5992224858009301,
+      "v": 0.010060789918368191,
+      "a": 0.0,
+      "g": 0.65
+    },
+    "maxVelocity": {
+      "val": 1080,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 540,
+      "uom": "deg/s^2"
+    }
+  },
+  "simSystemId": {
+    "feedBack": {
+      "p": 10.0,
+      "i": 0.0,
+      "d": 0.0
+    },
+    "feedForward": {
+      "s": 0.048722,
+      "v": 0.005486,
+      "a": 0.0,
+      "g": 0.04055
+    },
+    "maxVelocity": {
+      "val": 360,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 180,
+      "uom": "deg/s^2"
+    }
+  },
+  "length": {
+    "val": 0.234,
+    "uom": "m"
+  },
+  "lowerHardLimit": {
+    "val": 0,
+    "uom": "deg"
+  },
+  "upperHardLimit": {
+    "val": 125,
+    "uom": "deg"
+  },
+  "startingAngle": {
+    "val": 125,
+    "uom": "deg"
+  },
+  "lowerSoftLimit": {
+    "val": 0,
+    "uom": "deg"
+  },
+  "upperSoftLimit": {
+    "val": 120,
+    "uom": "deg"
+  },
+  "gearStages": "24:1",
+  "mass": {
+    "val": 1,
+    "uom": "lbs"
+  },
+  "horizontalZero": {
+    "val": 0,
+    "uom": "deg"
+  }
+}
diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_shooter.json b/src/main/deploy/rebuilt_robot/subsystems/intake/spintake_inner.json
similarity index 63%
rename from src/main/deploy/basic_robot/subsystems/example/yams_shooter.json
rename to src/main/deploy/rebuilt_robot/subsystems/intake/spintake_inner.json
index dc9a2c3b..e12c4798 100644
--- a/src/main/deploy/basic_robot/subsystems/example/yams_shooter.json
+++ b/src/main/deploy/rebuilt_robot/subsystems/intake/spintake_inner.json
@@ -1,9 +1,11 @@
 {
   "motorSetup": {
-    "name": "Shooter",
-    "controllerType": "spark",
-    "motorType": "KrakenX44",
+    "name": "spintake_inner",
+    "controllerType": "talonfx",
+    "motorType": "KrakenX60",
     "canId": 12,
+    "logLevel": "LOW",
+    "inverted": true,
     "robotToMotor": {
       "x": {
         "val": -5.872,
@@ -14,7 +16,7 @@
         "uom": "in"
       },
       "z": {
-        "val": 18.72,
+        "val": 14.466,
         "uom": "in"
       },
       "rotation": {
@@ -24,6 +26,7 @@
     }
   },
   "motorSystemId": {
+    "controlMode": "CLOSED_LOOP",
     "feedBack": {
       "p": 4,
       "i": 0.0,
@@ -33,6 +36,14 @@
       "s": 0.0,
       "v": 0.0,
       "a": 0.0
+    },
+    "maxVelocity": {
+      "val": 180,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 90,
+      "uom": "deg/s^2"
     }
   },
   "simSystemId": {
@@ -45,6 +56,14 @@
       "s": 0.0,
       "v": 0.0,
       "a": 0.0
+    },
+    "maxVelocity": {
+      "val": 180,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 90,
+      "uom": "deg/s^2"
     }
   },
   "lowerSoftLimit": {
@@ -55,10 +74,7 @@
     "val": 5000,
     "uom": "rpm"
   },
-  "gearing": [
-    3,
-    4
-  ],
+  "gearStages": "11:36",
   "mass": {
     "val": 2,
     "uom": "kg"
diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_arm.json b/src/main/deploy/rebuilt_robot/subsystems/intake/spintake_outer.json
similarity index 68%
rename from src/main/deploy/basic_robot/subsystems/example/yams_arm.json
rename to src/main/deploy/rebuilt_robot/subsystems/intake/spintake_outer.json
index 2f588f40..6cc7ab7c 100644
--- a/src/main/deploy/basic_robot/subsystems/example/yams_arm.json
+++ b/src/main/deploy/rebuilt_robot/subsystems/intake/spintake_outer.json
@@ -1,20 +1,22 @@
 {
   "motorSetup": {
-    "name": "Hood",
+    "name": "spintake_outer",
     "controllerType": "talonfx",
     "motorType": "KrakenX60",
     "canId": 13,
+    "logLevel": "LOW",
+    "inverted": true,
     "robotToMotor": {
       "x": {
-        "val": -8.2,
+        "val": -5.872,
         "uom": "in"
       },
       "y": {
-        "val": 10.104,
+        "val": 4.8,
         "uom": "in"
       },
       "z": {
-        "val": 15.786,
+        "val": 14.466,
         "uom": "in"
       },
       "rotation": {
@@ -24,6 +26,7 @@
     }
   },
   "motorSystemId": {
+    "controlMode": "CLOSED_LOOP",
     "feedBack": {
       "p": 4,
       "i": 0.0,
@@ -63,40 +66,21 @@
       "uom": "deg/s^2"
     }
   },
-  "length": {
-    "val": 0.234,
-    "uom": "m"
-  },
-  "lowerHardLimit": {
-    "val": 55,
-    "uom": "deg"
-  },
-  "upperHardLimit": {
-    "val": 100,
-    "uom": "deg"
-  },
-  "startingAngle": {
-    "val": 55,
-    "uom": "deg"
-  },
   "lowerSoftLimit": {
-    "val": 55,
-    "uom": "deg"
+    "val": 0,
+    "uom": "rpm"
   },
   "upperSoftLimit": {
-    "val": 85,
-    "uom": "deg"
+    "val": 5000,
+    "uom": "rpm"
   },
-  "gearing": [
-    3,
-    4
-  ],
+  "gearStages": "11:36",
   "mass": {
-    "val": 1,
-    "uom": "lbs"
+    "val": 2,
+    "uom": "kg"
   },
-  "horizontalZero": {
-    "val": 55,
-    "uom": "deg"
+  "radius": {
+    "val": 0.05,
+    "uom": "m"
   }
 }
diff --git a/src/main/deploy/rebuilt_robot/subsystems/launcher.json b/src/main/deploy/rebuilt_robot/subsystems/launcher.json
new file mode 100644
index 00000000..da7dc648
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/launcher.json
@@ -0,0 +1,16 @@
+{
+  "devices": [
+    {
+      "device": "yams_arm",
+      "file": "launcher/hood.json"
+    },
+    {
+      "device": "yams_shooter",
+      "file": "launcher/flywheel.json"
+    },
+    {
+      "device": "yams_turret",
+      "file": "launcher/turret.json"
+    }
+  ]
+}
diff --git a/src/main/deploy/rebuilt_robot/subsystems/launcher/flywheel.json b/src/main/deploy/rebuilt_robot/subsystems/launcher/flywheel.json
new file mode 100644
index 00000000..68c009b5
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/launcher/flywheel.json
@@ -0,0 +1,103 @@
+{
+  "motorSetup": {
+    "name": "flywheel",
+    "controllerType": "talonfx",
+    "motorType": "KrakenX60",
+    "canId": 16,
+    "inverted": true,
+    "logLevel": "LOW",
+    "currentLimit": {
+      "val": 80,
+      "uom": "amps"
+    },
+    "followers": [
+      {
+        "canId": 17,
+        "inverted": true
+      }
+    ],
+    "robotToMotor": {
+      "x": {
+        "val": -5.872,
+        "uom": "in"
+      },
+      "y": {
+        "val": 4.8,
+        "uom": "in"
+      },
+      "z": {
+        "val": 14.466,
+        "uom": "in"
+      },
+      "rotation": {
+        "val": 0.0,
+        "uom": "deg"
+      }
+    }
+  },
+  "motorSystemId": {
+    "feedBack": {
+      "p": 5,
+      "i": 0.0,
+      "d": 0.0
+    },
+    "closedLoopRamp": {
+      "val": 0,
+      "uom": "s"
+    },
+    "openLoopRamp": {
+      "val": 0,
+      "uom": "s"
+    },
+    "feedForward": {
+      "s": 0.066854,
+      "v": 2.1,
+      "a": 0.38848
+    },
+    "maxVelocity": {
+      "val": 180,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 90,
+      "uom": "deg/s^2"
+    }
+  },
+  "simSystemId": {
+    "feedBack": {
+      "p": 2,
+      "i": 0.0,
+      "d": 0.0
+    },
+    "feedForward": {
+      "s": 0.066854,
+      "v": 2.1962,
+      "a": 0.38848
+    },
+    "maxVelocity": {
+      "val": 180,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 90,
+      "uom": "deg/s^2"
+    }
+  },
+  "lowerSoftLimit": {
+    "val": 0,
+    "uom": "rpm"
+  },
+  "upperSoftLimit": {
+    "val": 5000,
+    "uom": "rpm"
+  },
+  "gearStages": "18:1",
+  "mass": {
+    "val": 5.1,
+    "uom": "lbs"
+  },
+  "radius": {
+    "val": 1.975,
+    "uom": "in"
+  }
+}
diff --git a/src/main/deploy/rebuilt_robot/subsystems/launcher/hood.json b/src/main/deploy/rebuilt_robot/subsystems/launcher/hood.json
new file mode 100644
index 00000000..2332e1e8
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/launcher/hood.json
@@ -0,0 +1,87 @@
+{
+  "motorSetup": {
+    "name": "hood",
+    "controllerType": "talonfx",
+    "motorType": "KrakenX44",
+    "canId": 19,
+    "logLevel": "LOW",
+    "currentLimit": {
+      "val": 60,
+      "uom": "amps"
+    }
+  },
+  "controlAlgorithm": "SIMPLE",
+  "motorSystemId": {
+    "feedBack": {
+      "p": 200,
+      "i": 0.0,
+      "d": 7
+    },
+    "feedForward": {
+      "s": 0.0,
+      "v": 0.0,
+      "a": 0.0
+    },
+    "maxVelocity": {
+      "val": 180,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 90,
+      "uom": "deg/s^2"
+    }
+  },
+  "simSystemId": {
+    "feedBack": {
+      "p": 9,
+      "i": 0.0,
+      "d": 0.0
+    },
+    "feedForward": {
+      "s": 0.0,
+      "v": 0.0,
+      "a": 0.0
+    },
+    "maxVelocity": {
+      "val": 180,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 90,
+      "uom": "deg/s^2"
+    }
+  },
+  "length": {
+    "val": 9.466,
+    "uom": "in"
+  },
+  "lowerHardLimit": {
+    "val": 30,
+    "uom": "deg"
+  },
+  "upperHardLimit": {
+    "val": 55,
+    "uom": "deg"
+  },
+  "startingAngle": {
+    "val": 30,
+    "uom": "deg"
+  },
+  "lowerSoftLimit": {
+    "val": 30,
+    "uom": "deg"
+  },
+  "upperSoftLimit": {
+    "val": 55,
+    "uom": "deg"
+  },
+  "gearStages": "1015:33",
+  "mass": {
+    "val": 0.25,
+    "uom": "lbs"
+  },
+  "horizontalZero": {
+    "val": 30,
+    "uom": "deg"
+  }
+}
diff --git a/src/main/deploy/rebuilt_robot/subsystems/launcher/turret.json b/src/main/deploy/rebuilt_robot/subsystems/launcher/turret.json
new file mode 100644
index 00000000..b1f551fc
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/launcher/turret.json
@@ -0,0 +1,100 @@
+{
+  "motorSetup": {
+    "name": "turret",
+    "controllerType": "talonfx",
+    "motorType": "KrakenX44",
+    "canId": 18,
+    "inverted": true,
+    "logLevel": "HIGH",
+    "canBus": "canivore",
+    "robotToMotor": {
+      "x": {
+        "val": -4.856,
+        "uom": "in"
+      },
+      "y": {
+        "val": 4.863,
+        "uom": "in"
+      },
+      "z": {
+        "val": 14.466,
+        "uom": "in"
+      },
+      "rotation": {
+        "val": 0.0,
+        "uom": "deg"
+      }
+    },
+    "movementPlane": "XY"
+  },
+  "controlAlgorithm": "SIMPLE",
+  "motorSystemId": {
+    "feedBack": {
+      "p": 1770.41578,
+      "i": 0.0,
+      "d": 1477.68211
+    },
+    "feedForward": {
+      "s": 31,
+      "v": 0,
+      "a": 1.94789461763
+    },
+    "maxVelocity": {
+      "val": 1080,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 10800,
+      "uom": "deg/s^2"
+    }
+  },
+  "simSystemId": {
+    "feedBack": {
+      "p": 8,
+      "i": 0.0,
+      "d": 8.0
+    },
+    "feedForward": {
+      "s": 0.030215,
+      "v": 0.00087341,
+      "a": 0.98956
+    },
+    "maxVelocity": {
+      "val": 1080,
+      "uom": "deg/s"
+    },
+    "maxAcceleration": {
+      "val": 10800,
+      "uom": "deg/s^2"
+    }
+  },
+  "lowerHardLimit": {
+    "val": -160,
+    "uom": "deg"
+  },
+  "upperHardLimit": {
+    "val": 160,
+    "uom": "deg"
+  },
+  "startingAngle": {
+    "val": 0,
+    "uom": "deg"
+  },
+  "lowerSoftLimit": {
+    "val": -150,
+    "uom": "deg"
+  },
+  "upperSoftLimit": {
+    "val": 150,
+    "uom": "deg"
+  },
+  "gearStages": "30:1",
+  "radius": {
+    "val": 5,
+    "uom": "in"
+  },
+  "mass": {
+    "val": 15,
+    "uom": "lb"
+  }
+}
diff --git a/src/main/deploy/basic_robot/subsystems/led_strip.json b/src/main/deploy/rebuilt_robot/subsystems/led_strip.json
similarity index 92%
rename from src/main/deploy/basic_robot/subsystems/led_strip.json
rename to src/main/deploy/rebuilt_robot/subsystems/led_strip.json
index 25c307af..2196ba12 100644
--- a/src/main/deploy/basic_robot/subsystems/led_strip.json
+++ b/src/main/deploy/rebuilt_robot/subsystems/led_strip.json
@@ -1,5 +1,5 @@
 {
-  "length": 30,
+  "length": 40,
   "dataPin": 0,
   "segments": [
     {
diff --git a/src/main/deploy/rebuilt_robot/subsystems/orchestra.json b/src/main/deploy/rebuilt_robot/subsystems/orchestra.json
new file mode 100644
index 00000000..bd01e06f
--- /dev/null
+++ b/src/main/deploy/rebuilt_robot/subsystems/orchestra.json
@@ -0,0 +1,39 @@
+{
+  "rioIds": [
+    9,
+    10,
+    11,
+    12,
+    13,
+    14,
+    15,
+    16,
+    17,
+    19
+  ],
+  "canivoreIds": [
+    1,
+    2,
+    3,
+    4,
+    5,
+    6,
+    7,
+    8,
+    18
+  ],
+  "music": [
+    {
+      "name": "raiders",
+      "path": "music/raiders.chrp"
+    },
+    {
+      "name": "mariachi",
+      "path": "music/mariachi.chrp"
+    },
+    {
+      "name": "sea2",
+      "path": "music/sea2.chrp"
+    }
+  ]
+}
diff --git a/src/main/deploy/basic_robot/yagsl_drivetrain.json b/src/main/deploy/rebuilt_robot/yagsl_drivetrain.json
similarity index 100%
rename from src/main/deploy/basic_robot/yagsl_drivetrain.json
rename to src/main/deploy/rebuilt_robot/yagsl_drivetrain.json
diff --git a/src/main/deploy/basic_robot/yagsl_swerve/controllerproperties.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/controllerproperties.json
similarity index 100%
rename from src/main/deploy/basic_robot/yagsl_swerve/controllerproperties.json
rename to src/main/deploy/rebuilt_robot/yagsl_swerve/controllerproperties.json
diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/backleft.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/backleft.json
similarity index 100%
rename from src/main/deploy/basic_robot/yagsl_swerve/modules/backleft.json
rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/backleft.json
diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/backright.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/backright.json
similarity index 100%
rename from src/main/deploy/basic_robot/yagsl_swerve/modules/backright.json
rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/backright.json
diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/frontleft.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/frontleft.json
similarity index 100%
rename from src/main/deploy/basic_robot/yagsl_swerve/modules/frontleft.json
rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/frontleft.json
diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/frontright.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/frontright.json
similarity index 100%
rename from src/main/deploy/basic_robot/yagsl_swerve/modules/frontright.json
rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/frontright.json
diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/physicalproperties.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/physicalproperties.json
similarity index 87%
rename from src/main/deploy/basic_robot/yagsl_swerve/modules/physicalproperties.json
rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/physicalproperties.json
index c921b000..bfc8d99c 100644
--- a/src/main/deploy/basic_robot/yagsl_swerve/modules/physicalproperties.json
+++ b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/physicalproperties.json
@@ -1,11 +1,11 @@
 {
   "conversionFactors": {
     "angle": {
-      "gearRatio": 12.8,
+      "gearRatio": 24,
       "factor": 0
     },
     "drive": {
-      "gearRatio": 8.14,
+      "gearRatio": 6.0,
       "diameter": 4,
       "factor": 0
     }
diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/pidfproperties.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/pidfproperties.json
similarity index 100%
rename from src/main/deploy/basic_robot/yagsl_swerve/modules/pidfproperties.json
rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/pidfproperties.json
diff --git a/src/main/deploy/basic_robot/yagsl_swerve/swervedrive.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/swervedrive.json
similarity index 100%
rename from src/main/deploy/basic_robot/yagsl_swerve/swervedrive.json
rename to src/main/deploy/rebuilt_robot/yagsl_swerve/swervedrive.json
diff --git a/src/main/deploy/robots.json b/src/main/deploy/robots.json
index 50eac16d..7710c0fc 100644
--- a/src/main/deploy/robots.json
+++ b/src/main/deploy/robots.json
@@ -4,13 +4,30 @@
     "basic_robot": {
       "id": "basic_robot",
       "robotClass": "frc.robot.example.ExampleRobot",
-      "simulate": true
+      "simulate": false
+    },
+    "tigershark": {
+      "id": "00:80:2F:38:90:61",
+      "robotClass": "frc.robot.tigershark.TigerShark",
+      "simulate": false,
+      "competition": false
+    },
+    "alpha": {
+      "id": "00:80:2F:33:17:DD",
+      "robotClass": "frc.robot.alpha.Alpha",
+      "competition": false,
+      "simulate": false
+    },
+    "rebuilt_robot": {
+      "id": "TBD",
+      "robotClass": "frc.robot.rebuilt.Rebuilt",
+      "simulate": true,
+      "competition": true
     },
     "baby_swerve": {
       "id": "00:80:2F:24:6D:74",
       "robotClass": "frc.robot.baby_swerve.BabySwerve",
-      "simulate": true,
-      "competition": true
+      "simulate": false
     }
   }
 }
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
index c8ece43a..59d1904f 100644
--- a/src/main/java/frc/robot/BuildConstants.java
+++ b/src/main/java/frc/robot/BuildConstants.java
@@ -3,14 +3,14 @@
 /** Automatically generated file containing build version information. */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "FRC5010Example";
+  public static final String MAVEN_NAME = "Rebuilt2026";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 81;
-  public static final String GIT_SHA = "e5bd64a909847009ebae0db75c2eb2a57ad49316";
-  public static final String GIT_DATE = "2026-01-26 17:57:48 EST";
-  public static final String GIT_BRANCH = "main";
-  public static final String BUILD_DATE = "2026-02-01 17:00:24 EST";
-  public static final long BUILD_UNIX_TIME = 1769983224737L;
+  public static final int GIT_REVISION = 626;
+  public static final String GIT_SHA = "45d944911aeffbc4d9740a395ac8d272a07552a8";
+  public static final String GIT_DATE = "2026-04-15 17:58:51 EDT";
+  public static final String GIT_BRANCH = "improvement/always-aim-turret";
+  public static final String BUILD_DATE = "2026-04-15 18:14:37 EDT";
+  public static final long BUILD_UNIX_TIME = 1776291277330L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 46c3435d..d4a77a02 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -148,8 +148,8 @@ public void disabledPeriodic() {
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
     m_robotContainer.setupDefaults();
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
diff --git a/src/main/java/frc/robot/baby_swerve/BabySwerve.java b/src/main/java/frc/robot/baby_swerve/BabySwerve.java
deleted file mode 100644
index ae8be2a3..00000000
--- a/src/main/java/frc/robot/baby_swerve/BabySwerve.java
+++ /dev/null
@@ -1,43 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.baby_swerve;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.example.DisplayValueSubsystem;
-import org.frc5010.common.arch.GenericRobot;
-import org.frc5010.common.config.ConfigConstants;
-import org.frc5010.common.constants.SwerveConstants;
-import org.frc5010.common.drive.GenericDrivetrain;
-import org.frc5010.common.sensors.Controller;
-
-/** This is an example robot class. */
-public class BabySwerve extends GenericRobot {
-  SwerveConstants swerveConstants;
-  GenericDrivetrain drivetrain;
-  DisplayValueSubsystem displayValueSubsystem = new DisplayValueSubsystem();
-
-  public BabySwerve(String directory) {
-    super(directory);
-    drivetrain = (GenericDrivetrain) getSubsystem(ConfigConstants.DRIVETRAIN);
-  }
-
-  @Override
-  public void configureButtonBindings(Controller driver, Controller operator) {}
-
-  @Override
-  public void setupDefaultCommands(Controller driver, Controller operator) {
-    drivetrain.setDefaultCommand(drivetrain.createDefaultCommand(driver));
-  }
-
-  @Override
-  public void initAutoCommands() {
-    drivetrain.setAutoBuilder();
-  }
-
-  @Override
-  public Command generateAutoCommand(Command autoCommand) {
-    return drivetrain.generateAutoCommand(autoCommand);
-  }
-}
diff --git a/src/main/java/frc/robot/example/ConfiguredMechanisms.java b/src/main/java/frc/robot/example/ConfiguredMechanisms.java
deleted file mode 100644
index 0082ec44..00000000
--- a/src/main/java/frc/robot/example/ConfiguredMechanisms.java
+++ /dev/null
@@ -1,80 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.example;
-
-import static edu.wpi.first.units.Units.Second;
-import static edu.wpi.first.units.Units.Volts;
-
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import org.frc5010.common.config.json.devices.ArmParser;
-import org.frc5010.common.config.json.devices.ElevatorParser;
-import org.frc5010.common.config.json.devices.PivotParser;
-import yams.mechanisms.positional.Arm;
-import yams.mechanisms.positional.Elevator;
-import yams.mechanisms.positional.Pivot;
-
-public class ConfiguredMechanisms extends SubsystemBase {
-  Elevator elevator;
-  Arm arm; // Assuming you have an Arm subsystem, otherwise remove this line
-  Pivot pivot; // Assuming you have a Pivot subsystem, otherwise remove this line
-
-  /** Creates a new ConfiguredElevator. */
-  public ConfiguredMechanisms() {
-    elevator = ElevatorParser.parse("mechanisms", "yams_elevator.json", this);
-    arm = ArmParser.parse("mechanisms", "yams_arm.json", this);
-    pivot = PivotParser.parse("mechanisms", "yams_pivot.json", this);
-  }
-
-  public void periodic() {
-    elevator.updateTelemetry();
-    arm.updateTelemetry();
-    pivot.updateTelemetry();
-  }
-
-  public void simulationPeriodic() {
-    elevator.simIterate();
-    arm.simIterate();
-    pivot.simIterate();
-  }
-
-  public Command elevCmd(double dutycycle) {
-    return elevator.set(dutycycle);
-  }
-
-  public Command setHeight(Distance height) {
-    return elevator.setHeight(height);
-  }
-
-  public Command sysIdElevator() {
-    return elevator.sysId(Volts.of(12), Volts.of(12).per(Second), Second.of(30));
-  }
-
-  public Command armCmd(double dutycycle) {
-    return arm.set(dutycycle);
-  }
-
-  public Command sysIdArm() {
-    return arm.sysId(Volts.of(3), Volts.of(3).per(Second), Second.of(30));
-  }
-
-  public Command setArmAngle(Angle angle) {
-    return arm.setAngle(angle);
-  }
-
-  public Command turretCmd(double dutycycle) {
-    return pivot.set(dutycycle);
-  }
-
-  public Command sysIdTurret() {
-    return pivot.sysId(Volts.of(3), Volts.of(3).per(Second), Second.of(30));
-  }
-
-  public Command setPivotAngle(Angle angle) {
-    return pivot.setAngle(angle);
-  }
-}
diff --git a/src/main/java/frc/robot/example/DisplayValueSubsystem.java b/src/main/java/frc/robot/example/DisplayValueSubsystem.java
deleted file mode 100644
index c7207255..00000000
--- a/src/main/java/frc/robot/example/DisplayValueSubsystem.java
+++ /dev/null
@@ -1,76 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.example;
-
-import static edu.wpi.first.units.Units.Degrees;
-
-import org.frc5010.common.arch.GenericSubsystem;
-import org.frc5010.common.telemetry.DisplayAngle;
-import org.frc5010.common.telemetry.DisplayBoolean;
-import org.frc5010.common.telemetry.DisplayDouble;
-import org.frc5010.common.telemetry.DisplayFloat;
-import org.frc5010.common.telemetry.DisplayLength;
-import org.frc5010.common.telemetry.DisplayLong;
-import org.frc5010.common.telemetry.DisplayString;
-import org.frc5010.common.telemetry.DisplayTime;
-
-/** Tests the classes in the {@link org.frc5010.common.telemetry} package that Display values */
-public class DisplayValueSubsystem extends GenericSubsystem {
-  DisplayAngle inputAngle;
-  DisplayAngle outputAngle;
-  DisplayBoolean inputBoolean;
-  DisplayBoolean outputBoolean;
-  DisplayDouble inputDouble;
-  DisplayDouble outputDouble;
-  DisplayFloat inputFloat;
-  DisplayFloat outputFloat;
-  DisplayLength inputLength;
-  DisplayLength outputLength;
-  DisplayLong inputLong;
-  DisplayLong outputLong;
-  DisplayString inputString;
-  DisplayString outputString;
-  DisplayTime inputTime;
-  DisplayTime outputTime;
-  DisplayAngle outAngle;
-
-  public DisplayValueSubsystem() {
-    super();
-    outAngle = DashBoard.makeInfoAngle("Out Angle");
-    outputAngle = DashBoard.makeDisplayAngle("OUTPUT_ANGLE");
-    outputBoolean = DashBoard.makeDisplayBoolean("OUTPUT_BOOLEAN");
-    outputDouble = DashBoard.makeDisplayDouble("OUTPUT_DOUBLE");
-    outputFloat = DashBoard.makeDisplayFloat("OUTPUT_FLOAT");
-    DashBoard.nextColumn("Config");
-    inputAngle = DashBoard.makeConfigAngle("INPUT_ANGLE");
-    inputBoolean = DashBoard.makeConfigBoolean("INPUT_BOOLEAN");
-    inputDouble = DashBoard.makeConfigDouble("INPUT_DOUBLE");
-    inputFloat = DashBoard.makeConfigFloat("INPUT_FLOAT");
-    DashBoard.nextColumn("Input");
-    inputLength = DashBoard.makeConfigLength("INPUT_LENGTH");
-    inputLong = DashBoard.makeConfigLong("INPUT_LONG");
-    inputString = DashBoard.makeConfigString("INPUT_STRING");
-    inputTime = DashBoard.makeConfigTime("INPUT_TIME");
-    DashBoard.nextColumn("Debug-Info");
-    outputLength = DashBoard.makeInfoLength("OUTPUT_LENGTH");
-    outputLong = DashBoard.makeInfoLong("OUTPUT_LONG");
-    outputString = DashBoard.makeInfoString("OUTPUT_STRING");
-    outputTime = DashBoard.makeInfoTime("OUTPUT_TIME");
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-    outputAngle.setAngle(inputAngle);
-    outputBoolean.setValue(inputBoolean.getValue());
-    outputDouble.setValue(inputDouble.getValue());
-    outputFloat.setValue(inputFloat.getValue());
-    outputLength.setLength(inputLength);
-    outputLong.setValue(inputLong.getValue());
-    outputString.setValue(inputString.getValue());
-    outputTime.setTime(inputTime);
-    outAngle.setAngle(Degrees.of(Math.random() * 360.0));
-  }
-}
diff --git a/src/main/java/frc/robot/example/ExampleRobot.java b/src/main/java/frc/robot/example/ExampleRobot.java
deleted file mode 100644
index 0c8873a9..00000000
--- a/src/main/java/frc/robot/example/ExampleRobot.java
+++ /dev/null
@@ -1,59 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.example;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.example.commands.ExampleCommands;
-import frc.robot.example.subsystems.ExampleSubsystem;
-import org.frc5010.common.arch.GenericRobot;
-import org.frc5010.common.config.ConfigConstants;
-import org.frc5010.common.constants.SwerveConstants;
-import org.frc5010.common.drive.GenericDrivetrain;
-import org.frc5010.common.sensors.Controller;
-
-/** This is an example robot class. */
-public class ExampleRobot extends GenericRobot {
-  SwerveConstants swerveConstants;
-  GenericDrivetrain drivetrain;
-  DisplayValueSubsystem displayValueSubsystem = new DisplayValueSubsystem();
-  ExampleSubsystem exampleSubsystem;
-  ExampleCommands exampleCommands;
-
-  public ExampleRobot(String directory) {
-    super(directory);
-    drivetrain = (GenericDrivetrain) subsystems.get(ConfigConstants.DRIVETRAIN);
-    exampleSubsystem = new ExampleSubsystem();
-    exampleCommands = new ExampleCommands(subsystems);
-  }
-
-  @Override
-  public void configureButtonBindings(Controller driver, Controller operator) {
-    exampleCommands.configureButtonBindings(driver, operator);
-  }
-
-  @Override
-  public void setupDefaultCommands(Controller driver, Controller operator) {
-    exampleCommands.setDefaultCommands(driver, operator);
-    drivetrain.setDefaultCommand(drivetrain.createDefaultCommand(driver));
-  }
-
-  @Override
-  public void initAutoCommands() {
-    drivetrain.setAutoBuilder();
-  }
-
-  @Override
-  public Command generateAutoCommand(Command autoCommand) {
-    return drivetrain.generateAutoCommand(autoCommand);
-  }
-
-  @Override
-  public void buildAutoCommands() {
-    super.buildAutoCommands();
-    selectableCommand.addOption("Do Nothing", Commands.none());
-    drivetrain.addAutoCommands(selectableCommand);
-  }
-}
diff --git a/src/main/java/frc/robot/example/commands/ExampleCommands.java b/src/main/java/frc/robot/example/commands/ExampleCommands.java
deleted file mode 100644
index 99bdb403..00000000
--- a/src/main/java/frc/robot/example/commands/ExampleCommands.java
+++ /dev/null
@@ -1,131 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.example.commands;
-
-import static edu.wpi.first.units.Units.Inches;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.example.subsystems.ExampleSubsystem;
-import java.util.Map;
-import org.frc5010.common.arch.GenericSubsystem;
-import org.frc5010.common.arch.StateMachine;
-import org.frc5010.common.arch.StateMachine.State;
-import org.frc5010.common.config.ConfigConstants;
-import org.frc5010.common.drive.GenericDrivetrain;
-import org.frc5010.common.sensors.Controller;
-import org.frc5010.common.telemetry.DisplayString;
-import org.frc5010.common.telemetry.DisplayValuesHelper;
-
-/** Add your docs here. */
-public class ExampleCommands {
-  private StateMachine stateMachine;
-  private DisplayString commandState;
-  private DisplayValuesHelper DisplayHelper;
-  private State intakeState;
-  private State lowState;
-  private State prepState;
-  private State readyState;
-  private ExampleSubsystem launcher;
-  private GenericDrivetrain drivetrain;
-  private Map<String, GenericSubsystem> subsystems;
-  private Translation2d target = new Translation2d(Inches.of(182.11), Inches.of(158.84));
-
-  private static enum LauncherState {
-    INTAKE,
-    LOW_SPEED,
-    PREP_SHOOT,
-    READY_TO_SHOOT
-  }
-
-  private LauncherState requestedState = LauncherState.INTAKE;
-
-  public ExampleCommands(Map<String, GenericSubsystem> subsystems) {
-    this.subsystems = subsystems;
-    DisplayHelper = new DisplayValuesHelper("LauncherCommands", "Values");
-    commandState = DisplayHelper.makeDisplayString("Launcher State");
-
-    launcher = (ExampleSubsystem) subsystems.get(ExampleSubsystem.class.getSimpleName());
-    drivetrain = (GenericDrivetrain) this.subsystems.get(ConfigConstants.DRIVETRAIN);
-
-    stateMachine = new StateMachine("LauncherStateMachine");
-    intakeState = stateMachine.addState("INTAKE", intakeStateCommand());
-    lowState = stateMachine.addState("LOW-SPEED", lowStateCommand());
-    prepState = stateMachine.addState("PREP-SHOOT", prepStateCommand());
-    readyState = stateMachine.addState("READY-TO-SHOOT", readyStateCommand());
-    stateMachine.setInitialState(intakeState);
-  }
-
-  public void setDefaultCommands(Controller driver, Controller operator) {
-    if (launcher != null) {
-      stateMachine.addRequirements(launcher);
-      launcher.setDefaultCommand(stateMachine);
-    }
-  }
-
-  public void configureButtonBindings(Controller driver, Controller operator) {
-    driver.createRightBumper().onTrue(shouldPrepCommand()).onFalse(shouldIntakeCommand());
-    driver.createLeftBumper().onTrue(shouldShootCommand()).onFalse(shouldPrepCommand());
-    driver.createAButton().onTrue(shouldIntakeCommand()).onFalse(shouldUseLowSpeed());
-
-    driver.createBButton().whileTrue(launcher.sysIdPivot());
-
-    lowState.switchTo(prepState).when(() -> requestedState == LauncherState.PREP_SHOOT);
-    prepState.switchTo(lowState).when(() -> requestedState == LauncherState.LOW_SPEED);
-
-    prepState.switchTo(readyState).when(() -> requestedState == LauncherState.READY_TO_SHOOT);
-    readyState.switchTo(prepState).when(() -> requestedState == LauncherState.PREP_SHOOT);
-
-    intakeState.switchTo(lowState).when(() -> requestedState == LauncherState.LOW_SPEED);
-    lowState.switchTo(intakeState).when(() -> requestedState == LauncherState.INTAKE);
-  }
-
-  public Command shouldPrepCommand() {
-    return Commands.runOnce(() -> requestedState = LauncherState.PREP_SHOOT);
-  }
-
-  public Command shouldUseLowSpeed() {
-    return Commands.runOnce(() -> requestedState = LauncherState.LOW_SPEED);
-  }
-
-  public Command shouldShootCommand() {
-    return Commands.runOnce(() -> requestedState = LauncherState.READY_TO_SHOOT);
-  }
-
-  public Command shouldIntakeCommand() {
-    return Commands.runOnce(() -> requestedState = LauncherState.INTAKE);
-  }
-
-  private Translation2d getTargetPose() {
-    return target.minus(drivetrain.getPoseEstimator().getCurrentPose().getTranslation());
-  }
-
-  private Command intakeStateCommand() {
-    return Commands.parallel(
-        Commands.runOnce(() -> commandState.setValue("Intake")),
-        launcher.stopTrackingCommand(),
-        launcher.intakeCommand());
-  }
-
-  private Command lowStateCommand() {
-    return Commands.parallel(
-        Commands.runOnce(() -> commandState.setValue("Low Speed")),
-        launcher.stopIntakeCommand(),
-        launcher.trackTargetCommand(() -> getTargetPose()));
-  }
-
-  private Command prepStateCommand() {
-    return Commands.parallel(
-        Commands.runOnce(() -> commandState.setValue("Prep")),
-        launcher.trackTargetCommand(() -> getTargetPose()));
-  }
-
-  private Command readyStateCommand() {
-    return Commands.parallel(
-        Commands.runOnce(() -> commandState.setValue("Ready")),
-        launcher.trackTargetCommand(() -> getTargetPose()));
-  }
-}
diff --git a/src/main/java/frc/robot/example/subsystems/ExampleIO.java b/src/main/java/frc/robot/example/subsystems/ExampleIO.java
deleted file mode 100644
index 89a53ee0..00000000
--- a/src/main/java/frc/robot/example/subsystems/ExampleIO.java
+++ /dev/null
@@ -1,79 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.example.subsystems;
-
-import static edu.wpi.first.units.Units.Degrees;
-
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import org.littletonrobotics.junction.AutoLog;
-
-/** Add your docs here. */
-public interface ExampleIO {
-  @AutoLog
-  public static class ExampleIOInputs {
-    public double shooterSpeedDesired = 0.0;
-    public double elevatorHeightDesired = 0.0;
-    public Angle hoodAngleDesired = Degrees.of(0.0);
-    public Angle turretAngleDesired = Degrees.of(0.0);
-
-    public double shooterSpeedActual = 0.0;
-    public double elevatorHeightActual = 0.0;
-    public Angle hoodAngleActual = Degrees.of(0.0);
-    public Angle turretAngleActual = Degrees.of(0.0);
-
-    public boolean shooterSpeedAtGoal = false;
-    public boolean elevatorHeightAtGoal = false;
-    public boolean hoodAngleAtGoal = false;
-    public boolean turretAngleAtGoal = false;
-
-    public double shooterSpeedError = 0.0;
-    public double elevatorHeightError = 0.0;
-    public double hoodAngleError = 0.0;
-    public double turretAngleError = 0.0;
-
-    public double hoodVelocity = 0.0;
-    public double turretVelocity = 0.0;
-    public double elevatorVelocity = 0.0;
-    public double shooterMotorOutput = 0.0;
-    public double elevatorMotorOutput = 0.0;
-  }
-
-  public default void updateInputs(ExampleIOInputs inputs) {}
-
-  public default void updateSimulation() {}
-
-  public void setPercentMotor(double output);
-
-  public Command setDutyCycle(double output);
-
-  public void runShooter(double speed);
-
-  public Command setUpperSpeed(AngularVelocity speed);
-
-  public Command setElevatorHeight(double height);
-
-  public void setHoodAngle(Angle angle);
-
-  public void setTurretRotation(Angle angle);
-
-  public AngularVelocity getShooterVelocity();
-
-  public Command sysIdShooter();
-
-  public Command sysIdArm();
-
-  public Command sysIdPivot();
-
-  public Command sysIdTurret();
-
-  public default Command addBallToRobot() {
-    return Commands.none();
-  }
-
-  public Command launchBall();
-}
diff --git a/src/main/java/frc/robot/example/subsystems/ExampleIOReal.java b/src/main/java/frc/robot/example/subsystems/ExampleIOReal.java
deleted file mode 100644
index 6bfd0a00..00000000
--- a/src/main/java/frc/robot/example/subsystems/ExampleIOReal.java
+++ /dev/null
@@ -1,117 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.example.subsystems;
-
-import static edu.wpi.first.units.Units.Seconds;
-import static edu.wpi.first.units.Units.Volts;
-
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import java.util.Map;
-import org.frc5010.common.arch.GenericSubsystem;
-import org.frc5010.common.motors.SystemIdentification;
-import org.frc5010.common.motors.function.AngularControlMotor;
-import org.frc5010.common.motors.function.PercentControlMotor;
-import org.frc5010.common.motors.function.VelocityControlMotor;
-import yams.mechanisms.positional.Arm;
-import yams.mechanisms.positional.Pivot;
-import yams.mechanisms.velocity.FlyWheel;
-
-/** Add your docs here. */
-public class ExampleIOReal implements ExampleIO {
-  protected Map<String, Object> devices;
-  protected PercentControlMotor percentMotor;
-  protected VelocityControlMotor controlledMotor;
-  protected AngularControlMotor angularMotor;
-  protected FlyWheel shooter;
-  protected Arm arm;
-  protected GenericSubsystem parent;
-  protected Pivot pivot;
-
-  public ExampleIOReal(Map<String, Object> devices, GenericSubsystem parent) {
-    this.devices = devices;
-    this.parent = parent;
-    this.percentMotor = (PercentControlMotor) devices.get("percent_motor");
-    this.controlledMotor = (VelocityControlMotor) devices.get("velocity_motor");
-    this.shooter = (FlyWheel) devices.get("Shooter");
-    this.arm = (Arm) devices.get("Hood");
-    this.pivot = (Pivot) devices.get("Turret");
-    this.angularMotor = (AngularControlMotor) devices.get("angular_motor");
-  }
-
-  @Override
-  public void updateInputs(ExampleIOInputs inputs) {
-
-    angularMotor.periodicUpdate();
-  }
-
-  @Override
-  public void setPercentMotor(double output) {
-    percentMotor.set(output);
-  }
-
-  @Override
-  public void runShooter(double speed) {
-    shooter.getMotor().setDutyCycle(speed);
-  }
-
-  @Override
-  public Command setUpperSpeed(AngularVelocity speed) {
-    return shooter.setSpeed(speed);
-  }
-
-  @Override
-  public Command setElevatorHeight(double height) {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'setElevatorHeight'");
-  }
-
-  @Override
-  public void setHoodAngle(Angle angle) {
-    arm.getMotorController().setPosition(angle);
-  }
-
-  @Override
-  public void setTurretRotation(Angle angle) {}
-
-  public AngularVelocity getShooterVelocity() {
-    return shooter.getSpeed();
-  }
-
-  public Command sysIdShooter() {
-    return SystemIdentification.getSysIdFullCommand(
-        SystemIdentification.rpmSysIdRoutine(shooter.getMotor(), parent.getName(), parent),
-        5,
-        3,
-        3);
-  }
-
-  public Command sysIdTurret() {
-    return SystemIdentification.getSysIdFullCommand(
-        SystemIdentification.angleSysIdRoutine(
-            pivot.getMotorController(), parent.getName(), parent),
-        5,
-        3,
-        3);
-  }
-
-  public Command sysIdArm() {
-    return arm.sysId(Volts.of(12), Volts.of(1).per(Seconds), Seconds.of(10));
-  }
-
-  public Command sysIdPivot() {
-    return pivot.sysId(Volts.of(12), Volts.of(1).per(Seconds), Seconds.of(10));
-  }
-
-  public Command launchBall() {
-    return shooter.set(0);
-  }
-
-  public Command setDutyCycle(double output) {
-    return Commands.runOnce(() -> percentMotor.set(output), parent);
-  }
-}
diff --git a/src/main/java/frc/robot/example/subsystems/ExampleIOSim.java b/src/main/java/frc/robot/example/subsystems/ExampleIOSim.java
deleted file mode 100644
index 3527c0a1..00000000
--- a/src/main/java/frc/robot/example/subsystems/ExampleIOSim.java
+++ /dev/null
@@ -1,115 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.example.subsystems;
-
-import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.Inches;
-import static edu.wpi.first.units.Units.Meters;
-import static edu.wpi.first.units.Units.MetersPerSecond;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import java.util.Map;
-import org.frc5010.common.arch.GenericSubsystem;
-import org.frc5010.common.drive.GenericDrivetrain;
-import org.frc5010.common.drive.swerve.YAGSLSwerveDrivetrain;
-import org.frc5010.lobbinloco.FRC5010BallOnTheFly;
-import org.littletonrobotics.junction.Logger;
-import swervelib.simulation.ironmaple.simulation.IntakeSimulation;
-import swervelib.simulation.ironmaple.simulation.IntakeSimulation.IntakeSide;
-import swervelib.simulation.ironmaple.simulation.SimulatedArena;
-import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceProjectile;
-import swervelib.simulation.ironmaple.simulation.seasonspecific.crescendo2024.NoteOnFly;
-import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly;
-
-/** Add your docs here. */
-public class ExampleIOSim extends ExampleIOReal {
-  protected IntakeSimulation intakeSimulation;
-  protected NoteOnFly noteOnFly;
-  protected RebuiltFuelOnFly fuelOnFly;
-  protected GamePieceProjectile gamePieceProjectile;
-
-  public ExampleIOSim(Map<String, Object> devices, GenericSubsystem parent) {
-    super(devices, parent);
-    intakeSimulation =
-        IntakeSimulation.InTheFrameIntake(
-            "FRC5010Ball",
-            GenericDrivetrain.getMapleSimDrive().get(),
-            Inches.of(24.25),
-            IntakeSide.FRONT,
-            1);
-  }
-
-  @Override
-  public void updateSimulation() {
-    angularMotor.simulationUpdate();
-  }
-
-  @Override
-  public void setPercentMotor(double speed) {
-    if (speed > 0.0 && !noteIsInsideIntake().getAsBoolean()) {
-      intakeSimulation.startIntake();
-    } else {
-      intakeSimulation.stopIntake();
-    }
-    super.setPercentMotor(speed);
-  }
-
-  public Trigger obtainedGamePieceToScore() {
-    return new Trigger(
-        () -> {
-          return RobotBase.isSimulation()
-              ? intakeSimulation.getGamePiecesAmount() == 1
-                  && intakeSimulation.obtainGamePieceFromIntake()
-              : false;
-        });
-  }
-
-  public Trigger noteIsInsideIntake() {
-    return new Trigger(
-        () -> {
-          return RobotBase.isSimulation() ? intakeSimulation.getGamePiecesAmount() > 0 : false;
-        });
-  }
-
-  public Command addBallToRobot() {
-    return Commands.runOnce(() -> intakeSimulation.addGamePieceToIntake());
-  }
-
-  @Override
-  public Command launchBall() {
-    return super.launchBall()
-        .alongWith(
-            Commands.runOnce(
-                () -> {
-                  if (RobotBase.isSimulation()) {
-                    Pose2d worldPose = YAGSLSwerveDrivetrain.getSwerveDrive().getPose();
-                    gamePieceProjectile =
-                        new FRC5010BallOnTheFly(
-                                worldPose.getTranslation(),
-                                controlledMotor
-                                    .getRobotToMotor()
-                                    .getTranslation()
-                                    .toTranslation2d(),
-                                YAGSLSwerveDrivetrain.getSwerveDrive().getFieldVelocity(),
-                                worldPose.getRotation(),
-                                Meters.of(0.45),
-                                MetersPerSecond.of(10),
-                                Degrees.of(55))
-                            .withProjectileTrajectoryDisplayCallBack(
-                                (pose3ds) -> {
-                                  Logger.recordOutput(
-                                      parent.getName() + "/GPTrajectory",
-                                      pose3ds.toArray(Pose3d[]::new));
-                                });
-                    SimulatedArena.getInstance().addGamePieceProjectile(gamePieceProjectile);
-                  }
-                }));
-  }
-}
diff --git a/src/main/java/frc/robot/example/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/example/subsystems/ExampleSubsystem.java
deleted file mode 100644
index b8398c82..00000000
--- a/src/main/java/frc/robot/example/subsystems/ExampleSubsystem.java
+++ /dev/null
@@ -1,179 +0,0 @@
-package frc.robot.example.subsystems;
-
-import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.Inches;
-
-import com.revrobotics.spark.SparkMax;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.example.subsystems.ExampleIO.ExampleIOInputs;
-import java.util.function.Supplier;
-import org.frc5010.common.arch.GenericSubsystem;
-import org.frc5010.common.constants.GenericPID;
-import org.frc5010.common.constants.MotorFeedFwdConstants;
-import org.frc5010.common.motors.MotorConstants.Motor;
-import org.frc5010.common.motors.MotorFactory;
-import org.frc5010.common.motors.function.AngularControlMotor;
-import org.frc5010.common.sensors.absolute_encoder.RevAbsoluteEncoder;
-
-public class ExampleSubsystem extends GenericSubsystem {
-  protected ExampleIO io;
-  protected ExampleIOInputs inputs = new ExampleIOInputs();
-  protected int scoredNotes = 0;
-  protected Rotation2d rotation = new Rotation2d(Degrees.of(180));
-
-  public ExampleSubsystem() {
-    super("example.json");
-    devices.put("angular_motor", angularControlledMotor());
-    if (RobotBase.isSimulation()) {
-      io = new ExampleIOSim(devices, this);
-    } else {
-      io = new ExampleIOReal(devices, this);
-    }
-  }
-
-  public Command trackTargetCommand(Supplier<Translation2d> targetSupplier) {
-    return Commands.run(
-        () -> {
-          Translation2d targetPose = targetSupplier.get();
-          targetPose.getNorm();
-          io.setTurretRotation(targetPose.getAngle().getMeasure());
-          io.setHoodAngle(Degrees.of(45));
-        });
-  }
-
-  public Command stopTrackingCommand() {
-    return Commands.runOnce(
-        () -> {
-          io.setTurretRotation(Degrees.of(0));
-          io.setHoodAngle(Degrees.of(0));
-        });
-  }
-
-  public Command intakeCommand() {
-    return Commands.runOnce(() -> io.setPercentMotor(0.5), this);
-  }
-
-  public Command stopIntakeCommand() {
-    return Commands.runOnce(() -> io.setPercentMotor(0.0), this);
-  }
-
-  public Command sysIdShooter() {
-    return io.sysIdShooter();
-  }
-
-  public Command addBallToRobot() {
-    return io.addBallToRobot();
-  }
-
-  public Command launchBall() {
-    return io.launchBall();
-  }
-
-  public Command sysIdArm() {
-    return io.sysIdArm();
-  }
-
-  public Command sysIdPivot() {
-    return io.sysIdPivot();
-  }
-
-  public Command sysIdTurret() {
-    return io.sysIdTurret();
-  }
-
-  @Override
-  public void periodic() {
-    super.periodic();
-    io.updateInputs(inputs);
-  }
-
-  @Override
-  public void simulationPeriodic() {
-    super.simulationPeriodic();
-    io.updateSimulation();
-  }
-
-  public AngularControlMotor angularControlledMotor() {
-    AngularControlMotor angularMotor =
-        new AngularControlMotor(
-                MotorFactory.Spark(13, Motor.Neo), "angular", getDisplayValuesHelper())
-            .setupSimulatedMotor(
-                (5.0 * 68.0 / 24.0) * (80.0 / 24.0),
-                Units.lbsToKilograms(22),
-                Inches.of(19),
-                Degrees.of(0),
-                Degrees.of(360),
-                false,
-                0,
-                Degrees.of(0),
-                false,
-                0.1)
-            .setVisualizer(mechanismSimulation, new Pose3d(0.75, 0, 0.25, new Rotation3d()));
-    angularMotor.setEncoder(new RevAbsoluteEncoder((SparkMax) angularMotor.getMotor(), 360));
-    angularMotor.setValues(new GenericPID(0.01, 0.000025, 0.003));
-    angularMotor.setMotorFeedFwd(new MotorFeedFwdConstants(0.0, 0.01, 0.0, false));
-    angularMotor.setIZone(3);
-    angularMotor.setOutputRange(-12, 12);
-    return angularMotor;
-  }
-
-  // public Command setVelocityControlMotorReference(DoubleSupplier reference) {
-  //   return Commands.runOnce(
-  //       () -> {
-  //         double speed = reference.getAsDouble();
-  //         if (speed <= 0.0 && !noteIsInsideIntake().getAsBoolean()) {
-  //           controlledMotor.setReference(speed);
-  //         } else if (speed > 3000
-  //             && noteIsInsideIntake().getAsBoolean()
-  //             && obtainedGamePieceToScore().getAsBoolean()) {
-  //           controlledMotor.setReference(speed);
-  //           if (RobotBase.isSimulation()) {
-  //             Pose2d worldPose = YAGSLSwerveDrivetrain.getSwerveDrive().getPose();
-  //             gamePieceProjectile =
-  //                 new ReefscapeAlgaeOnFly(
-  //                     worldPose.getTranslation(),
-  //                     controlledMotor.getRobotToMotor().getTranslation().toTranslation2d(),
-  //                     YAGSLSwerveDrivetrain.getSwerveDrive().getFieldVelocity(),
-  //                     worldPose.getRotation(),
-  //                     Meters.of(0.45),
-  //                     MetersPerSecond.of(speed / 6000 * 20),
-  //                     Degrees.of(55));
-  //             SimulatedArena.getInstance().addGamePieceProjectile(gamePieceProjectile);
-  //           }
-  //         } else if (speed < 3000
-  //             && speed > 1000
-  //             && noteIsInsideIntake().getAsBoolean()
-  //             && obtainedGamePieceToScore().getAsBoolean()) {
-  //           controlledMotor.setReference(speed);
-  //           if (RobotBase.isSimulation()) {
-  //             Pose2d worldPose = YAGSLSwerveDrivetrain.getSwerveDrive().getPose();
-  //             gamePieceProjectile =
-  //                 new ReefscapeAlgaeOnFly(
-  //                     worldPose.getTranslation(),
-  //                     controlledMotor.getRobotToMotor().getTranslation().toTranslation2d(),
-  //                     YAGSLSwerveDrivetrain.getSwerveDrive().getFieldVelocity(),
-  //                     worldPose.getRotation(),
-  //                     Meters.of(0.45),
-  //                     MetersPerSecond.of(speed / 6000 * 20),
-  //                     Degrees.of(55));
-  //             SimulatedArena.getInstance().addGamePieceProjectile(gamePieceProjectile);
-  //           }
-  //         } else {
-  //           controlledMotor.setReference(speed);
-  //         }
-  //       },
-  //       this);
-  // }
-
-  // public Command setAngularMotorReference(DoubleSupplier reference) {
-  //   return Commands.runOnce(() -> angularMotor.setReference(reference.getAsDouble()), this);
-  // }
-
-}
diff --git a/src/main/java/frc/robot/rebuilt/Constants.java b/src/main/java/frc/robot/rebuilt/Constants.java
new file mode 100644
index 00000000..fd6be8e9
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/Constants.java
@@ -0,0 +1,64 @@
+package frc.robot.rebuilt;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Inch;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
+import frc.robot.rebuilt.subsystems.Climb.Climb;
+
+public class Constants {
+  public static final String INDEXER = Indexer.class.getSimpleName();
+  public static final String CLIMB = Climb.class.getSimpleName();
+  public static final String INTAKE = Intake.class.getSimpleName();
+  public static final String LAUNCHER = Launcher.class.getSimpleName();
+  /** Defines the maxinum distance for the climb */
+  public static class ClimbConstants {
+    public static final Distance MAX = Inch.of(27);
+  }
+
+  public static class Launcher {
+    public static final double SHOOTER_TOLERANCE_RPM = 50.0;
+    public static final double HOOD_ANGLE_TOLERANCE_DEGREES = 3.5;
+    public static final double TURRET_ANGLE_TOLERANCE_DEGREES = 5.0;
+
+    public static final Angle LOW_HOOD_ANGLE = Degrees.of(31);
+    public static final AngularVelocity LOW_FLYWHEEL_RPM = RotationsPerSecond.of(1);
+
+    public static final Angle HUB_HOOD_ANGLE = LOW_HOOD_ANGLE;
+    public static final AngularVelocity HUB_FLYWHEEL_RPM = RotationsPerSecond.of(1.25);
+
+    public static final Angle TOWER_HOOD_ANGLE = Degrees.of(40);
+    public static final AngularVelocity TOWER_FLYWHEEL_RPM = RotationsPerSecond.of(1.5);
+
+    public static final Angle TURRET_FORWARD = Degrees.of(0);
+    public static final AngularVelocity FWD_FLYWHEEL_RPM = LOW_FLYWHEEL_RPM;
+    public static final Angle FWD_HOOD_ANGLE = LOW_HOOD_ANGLE;
+  }
+
+  public static class Indexer {
+    public static final double SPINDEXER_SPEED = 0.90;
+    public static final double TRANSFER_SPEED = 1.0;
+    public static final double TRANSFER_CHURN = 0.25;
+  }
+
+  public static class Intake {
+    public static final double HOPPER_GO_OUT = -0.3;
+    public static final double HOPPER_GO_IN = 0.2;
+    public static final double INTAKE_IN = 1.0;
+    public static final double INTAKE_INNER_IN = 0.3;
+    public static final double INTAKE_AUTO = 1.0;
+    public static final double INTAKE_DEADZONE = 0.25;
+    public static final double INTAKE_CHURN = -0.25;
+    public static final double INTAKE_MAX_IN = 0.9;
+    public static final double INTAKE_MAX_OUT = -0.9;
+    public static final double HOPPER_ANGLE_TOLERANCE = 3;
+    public static final double HOPPER_STALL_TIME = 0.3;
+    public static final Angle HOPPER_RETRACTED_ANGLE = Degrees.of(120);
+    public static final Angle HOPPER_DEPLOYED_ANGLE = Degrees.of(0);
+    public static final Angle HOPPER_ANGLED = Degrees.of(45);
+    public static final double HOPPER_STALL_CURRENT_THRESHOLD = 40.0;
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/FieldConstants.java b/src/main/java/frc/robot/rebuilt/FieldConstants.java
new file mode 100644
index 00000000..9412d55c
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/FieldConstants.java
@@ -0,0 +1,406 @@
+// Copyright (c) 2025-2026 Littleton Robotics
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.rebuilt;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Distance;
+import org.frc5010.common.vision.AprilTags;
+
+/**
+ * Contains information for location of field element and other useful reference points.
+ *
+ * <p>NOTE: All constants are defined relative to the field coordinate system, and from the
+ * perspective of the blue alliance station
+ */
+public class FieldConstants {
+
+  public static final FieldType fieldType = FieldType.ANDYMARK;
+
+  // AprilTag related constants
+  public static final int aprilTagCount = AprilTags.aprilTagFieldLayout.getTags().size();
+  public static final double aprilTagWidth = Units.inchesToMeters(6.5);
+
+  // Field dimensions
+  public static final double fieldLength = AprilTags.aprilTagFieldLayout.getFieldLength();
+  public static final double fieldWidth = AprilTags.aprilTagFieldLayout.getFieldWidth();
+  public static final Distance FIELD_LENGTH = Meters.of(fieldLength);
+  public static final Distance FIELD_WIDTH = Meters.of(fieldWidth);
+  public static final Translation2d TRENCH_HALF_WIDTH =
+      new Translation2d(Meters.of(1.5), Meters.of(0));
+  /**
+   * Officially defined and relevant vertical lines found on the field (defined by X-axis offset)
+   */
+  public static class LinesVertical {
+    public static final double center = fieldLength / 2.0;
+    public static final double starting = AprilTags.aprilTagFieldLayout.getTagPose(26).get().getX();
+    public static final double allianceZone = starting;
+    public static final double hubCenter =
+        AprilTags.aprilTagFieldLayout.getTagPose(26).get().getX() + Hub.width / 2.0;
+    public static final double neutralZoneNear = center - Units.inchesToMeters(120);
+    public static final double neutralZoneFar = center + Units.inchesToMeters(120);
+    public static final double oppHubCenter =
+        AprilTags.aprilTagFieldLayout.getTagPose(4).get().getX() + Hub.width / 2.0;
+    public static final double oppAllianceZone =
+        AprilTags.aprilTagFieldLayout.getTagPose(10).get().getX();
+  }
+
+  /**
+   * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset)
+   *
+   * <p>NOTE: The field element start and end are always left to right from the perspective of the
+   * alliance station
+   */
+  public static class LinesHorizontal {
+
+    public static final double center = fieldWidth / 2.0;
+
+    // Right of hub
+    public static final double rightBumpStart = Hub.nearRightCorner.getY();
+    public static final double rightBumpEnd = rightBumpStart - RightBump.width;
+    public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0);
+    public static final double rightTrenchOpenEnd = 0;
+
+    // Left of hub
+    public static final double leftBumpEnd = Hub.nearLeftCorner.getY();
+    public static final double leftBumpStart = leftBumpEnd + LeftBump.width;
+    public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0);
+    public static final double leftTrenchOpenStart = fieldWidth;
+  }
+
+  /** Hub related constants */
+  public static class Hub {
+
+    // Dimensions
+    public static final double width = Units.inchesToMeters(47.0);
+    public static final double height =
+        Units.inchesToMeters(72.0); // includes the catcher at the top
+    public static final double innerWidth = Units.inchesToMeters(41.7);
+    public static final double innerHeight = Units.inchesToMeters(56.5);
+
+    // Relevant reference points on alliance side
+    public static final Translation3d topCenterPoint =
+        new Translation3d(
+            AprilTags.aprilTagFieldLayout.getTagPose(26).get().getX() + width / 2.0,
+            fieldWidth / 2.0,
+            height);
+    public static final Translation3d innerCenterPoint =
+        new Translation3d(
+            AprilTags.aprilTagFieldLayout.getTagPose(26).get().getX() + width / 2.0,
+            fieldWidth / 2.0,
+            innerHeight);
+
+    public static final Translation2d nearLeftCorner =
+        new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0);
+    public static final Translation2d nearRightCorner =
+        new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0);
+    public static final Translation2d farLeftCorner =
+        new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0);
+    public static final Translation2d farRightCorner =
+        new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0);
+
+    // Relevant reference points on the opposite side
+    public static final Translation3d oppTopCenterPoint =
+        new Translation3d(
+            AprilTags.aprilTagFieldLayout.getTagPose(4).get().getX() + width / 2.0,
+            fieldWidth / 2.0,
+            height);
+    public static final Translation2d oppNearLeftCorner =
+        new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0);
+    public static final Translation2d oppNearRightCorner =
+        new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0);
+    public static final Translation2d oppFarLeftCorner =
+        new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0);
+    public static final Translation2d oppFarRightCorner =
+        new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0);
+
+    // Hub faces
+    public static final Pose2d nearFace =
+        AprilTags.aprilTagFieldLayout.getTagPose(26).get().toPose2d();
+    public static final Translation2d centerFace =
+        new Translation2d(nearFace.getX(), fieldWidth / 2.0);
+    public static final Pose2d farFace =
+        AprilTags.aprilTagFieldLayout.getTagPose(20).get().toPose2d();
+    public static final Pose2d rightFace =
+        AprilTags.aprilTagFieldLayout.getTagPose(18).get().toPose2d();
+    public static final Pose2d leftFace =
+        AprilTags.aprilTagFieldLayout.getTagPose(21).get().toPose2d();
+  }
+
+  /** Left Bump related constants */
+  public static class LeftBump {
+
+    // Dimensions
+    public static final double width = Units.inchesToMeters(73.0);
+    public static final double height = Units.inchesToMeters(6.513);
+    public static final double depth = Units.inchesToMeters(44.4);
+
+    // Relevant reference points on alliance side
+    public static final Translation2d nearLeftCorner =
+        new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255));
+    public static final Translation2d nearRightCorner = Hub.nearLeftCorner;
+    public static final Translation2d farLeftCorner =
+        new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255));
+    public static final Translation2d farRightCorner = Hub.farLeftCorner;
+
+    // Relevant reference points on opposing side
+    public static final Translation2d oppNearLeftCorner =
+        new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255));
+    public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner;
+    public static final Translation2d oppFarLeftCorner =
+        new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255));
+    public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner;
+  }
+
+  /** Right Bump related constants */
+  public static class RightBump {
+    // Dimensions
+    public static final double width = Units.inchesToMeters(73.0);
+    public static final double height = Units.inchesToMeters(6.513);
+    public static final double depth = Units.inchesToMeters(44.4);
+
+    // Relevant reference points on alliance side
+    public static final Translation2d nearLeftCorner =
+        new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255));
+    public static final Translation2d nearRightCorner = Hub.nearLeftCorner;
+    public static final Translation2d farLeftCorner =
+        new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255));
+    public static final Translation2d farRightCorner = Hub.farLeftCorner;
+
+    // Relevant reference points on opposing side
+    public static final Translation2d oppNearLeftCorner =
+        new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255));
+    public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner;
+    public static final Translation2d oppFarLeftCorner =
+        new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255));
+    public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner;
+  }
+
+  /** Left Trench related constants */
+  public static class LeftTrench {
+    // Dimensions
+    public static final double width = Units.inchesToMeters(65.65);
+    public static final double depth = Units.inchesToMeters(47.0);
+    public static final double height = Units.inchesToMeters(40.25);
+    public static final double openingWidth = Units.inchesToMeters(50.34);
+    public static final double openingHeight = Units.inchesToMeters(22.25);
+
+    // Relevant reference points on alliance side
+    public static final Translation3d openingTopLeft =
+        new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight);
+    public static final Translation3d openingTopRight =
+        new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight);
+
+    // Relevant reference points on opposing side
+    public static final Translation3d oppOpeningTopLeft =
+        new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight);
+    public static final Translation3d oppOpeningTopRight =
+        new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight);
+  }
+
+  public static class TrenchZoneTop {
+    public static Translation2d nearAlliance = LeftTrench.openingTopRight.toTranslation2d();
+    public static Translation2d nearAllianceLeftDanger =
+        LeftTrench.openingTopRight.toTranslation2d().minus(TRENCH_HALF_WIDTH);
+    public static Translation2d nearAllianceRightDanger =
+        LeftTrench.openingTopRight.toTranslation2d().plus(TRENCH_HALF_WIDTH);
+
+    public static Translation2d oppAlliance = LeftTrench.oppOpeningTopRight.toTranslation2d();
+    public static Translation2d oppAllianceLeftDanger =
+        LeftTrench.oppOpeningTopRight.toTranslation2d().minus(TRENCH_HALF_WIDTH);
+    public static Translation2d oppAllianceRightDanger =
+        LeftTrench.oppOpeningTopRight.toTranslation2d().plus(TRENCH_HALF_WIDTH);
+  }
+
+  public static class TrenchZoneBottom {
+    public static Translation2d nearAlliance = RightTrench.openingTopRight.toTranslation2d();
+    public static Translation2d nearAllianceLeftDanger =
+        RightTrench.openingTopRight.toTranslation2d().minus(TRENCH_HALF_WIDTH);
+    public static Translation2d nearAllianceRightDanger =
+        RightTrench.openingTopRight.toTranslation2d().plus(TRENCH_HALF_WIDTH);
+
+    public static Translation2d oppAlliance = RightTrench.openingTopLeft.toTranslation2d();
+    public static Translation2d oppAllianceLeftDanger =
+        RightTrench.oppOpeningTopLeft.toTranslation2d().minus(TRENCH_HALF_WIDTH);
+    public static Translation2d oppAllianceRightDanger =
+        RightTrench.oppOpeningTopLeft.toTranslation2d().plus(TRENCH_HALF_WIDTH);
+  }
+
+  public static class RightTrench {
+
+    // Dimensions
+    public static final double width = Units.inchesToMeters(65.65);
+    public static final double depth = Units.inchesToMeters(47.0);
+    public static final double height = Units.inchesToMeters(40.25);
+    public static final double openingWidth = Units.inchesToMeters(50.34);
+    public static final double openingHeight = Units.inchesToMeters(22.25);
+
+    // Relevant reference points on alliance side
+    public static final Translation3d openingTopLeft =
+        new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight);
+    public static final Translation3d openingTopRight =
+        new Translation3d(LinesVertical.hubCenter, 0, openingHeight);
+
+    // Relevant reference points on opposing side
+    public static final Translation3d oppOpeningTopLeft =
+        new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight);
+    public static final Translation3d oppOpeningTopRight =
+        new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight);
+  }
+
+  /** Tower related constants */
+  public static class Tower {
+    // Dimensions
+    public static final double width = Units.inchesToMeters(49.25);
+    public static final double depth = Units.inchesToMeters(45.0);
+    public static final double height = Units.inchesToMeters(78.25);
+    public static final double innerOpeningWidth = Units.inchesToMeters(32.250);
+    public static final double frontFaceX = Units.inchesToMeters(43.51);
+
+    public static final double uprightHeight = Units.inchesToMeters(72.1);
+
+    // Rung heights from the floor
+    public static final double lowRungHeight = Units.inchesToMeters(27.0);
+    public static final double midRungHeight = Units.inchesToMeters(45.0);
+    public static final double highRungHeight = Units.inchesToMeters(63.0);
+
+    // Relevant reference points on alliance side
+    public static final Translation2d centerPoint =
+        new Translation2d(frontFaceX, AprilTags.aprilTagFieldLayout.getTagPose(31).get().getY());
+    public static final Pose2d face =
+        new Pose2d(
+            frontFaceX,
+            AprilTags.aprilTagFieldLayout.getTagPose(31).get().getY(),
+            AprilTags.aprilTagFieldLayout.getTagPose(31).get().getRotation().toRotation2d());
+    public static final Translation2d leftUpright =
+        new Translation2d(
+            frontFaceX,
+            (AprilTags.aprilTagFieldLayout.getTagPose(31).get().getY())
+                + innerOpeningWidth / 2
+                + Units.inchesToMeters(0.75));
+    public static final Translation2d rightUpright =
+        new Translation2d(
+            frontFaceX,
+            (AprilTags.aprilTagFieldLayout.getTagPose(31).get().getY())
+                - innerOpeningWidth / 2
+                - Units.inchesToMeters(0.75));
+
+    // Relevant reference points on opposing side
+    public static final Translation2d oppCenterPoint =
+        new Translation2d(
+            fieldLength - frontFaceX, AprilTags.aprilTagFieldLayout.getTagPose(15).get().getY());
+    public static final Translation2d oppLeftUpright =
+        new Translation2d(
+            fieldLength - frontFaceX,
+            (AprilTags.aprilTagFieldLayout.getTagPose(15).get().getY())
+                + innerOpeningWidth / 2
+                + Units.inchesToMeters(0.75));
+    public static final Translation2d oppRightUpright =
+        new Translation2d(
+            fieldLength - frontFaceX,
+            (AprilTags.aprilTagFieldLayout.getTagPose(15).get().getY())
+                - innerOpeningWidth / 2
+                - Units.inchesToMeters(0.75));
+  }
+
+  public static class Depot {
+    // Dimensions
+    public static final double width = Units.inchesToMeters(42.0);
+    public static final double depth = Units.inchesToMeters(27.0);
+    public static final double height = Units.inchesToMeters(1.125);
+    public static final double distanceFromCenterY = Units.inchesToMeters(75.93);
+
+    // Relevant reference points on alliance side
+    public static final Translation3d depotCenter =
+        new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height);
+    public static final Translation3d leftCorner =
+        new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height);
+    public static final Translation3d rightCorner =
+        new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height);
+  }
+
+  public static class Outpost {
+    // Dimensions
+    public static final double width = Units.inchesToMeters(31.8);
+    public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1);
+    public static final double height = Units.inchesToMeters(7.0);
+
+    // Relevant reference points on alliance side
+    public static final Translation2d centerPoint =
+        new Translation2d(0, AprilTags.aprilTagFieldLayout.getTagPose(29).get().getY());
+  }
+
+  public enum FieldType {
+    ANDYMARK("andymark"),
+    WELDED("welded");
+
+    private final String jsonFolder;
+
+    FieldType(String jsonFolder) {
+      this.jsonFolder = jsonFolder;
+    }
+
+    public String getJsonFolder() {
+      return jsonFolder;
+    }
+  }
+
+  //   public enum AprilTagLayoutType {
+  //     OFFICIAL("2026-official"),
+  //     NONE("2026-none");
+
+  //     private final String name;
+  //     private volatile AprilTagFieldLayout layout;
+  //     private volatile String layoutString;
+
+  //     AprilTagLayoutType(String name) {
+  //       this.name = name;
+  //     }
+
+  //     public AprilTagFieldLayout getLayout() {
+  //       if (layout == null) {
+  //         synchronized (this) {
+  //           if (layout == null) {
+  //             try {
+  //               Path p =
+  //                   Constants.disableHAL
+  //                       ? Path.of(
+  //                           "src",
+  //                           "main",
+  //                           "deploy",
+  //                           "apriltags",
+  //                           fieldType.getJsonFolder(),
+  //                           name + ".json")
+  //                       : Path.of(
+  //                           Filesystem.getDeployDirectory().getPath(),
+  //                           "apriltags",
+  //                           fieldType.getJsonFolder(),
+  //                           name + ".json");
+  //               layout = new AprilTagFieldLayout(p);
+  //               layoutString = new ObjectMapper().writeValueAsString(layout);
+  //             } catch (IOException e) {
+  //               throw new RuntimeException(e);
+  //             }
+  //           }
+  //         }
+  //       }
+  //       return layout;
+  //     }
+
+  //     public String getLayoutString() {
+  //       if (layoutString == null) {
+  //         getLayout();
+  //       }
+  //       return layoutString;
+  //     }
+  //   }
+}
diff --git a/src/main/java/frc/robot/rebuilt/HubTracker.java b/src/main/java/frc/robot/rebuilt/HubTracker.java
new file mode 100644
index 00000000..33fb8045
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/HubTracker.java
@@ -0,0 +1,193 @@
+package frc.robot.rebuilt;
+
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import java.util.Optional;
+
+public class HubTracker {
+  /**
+   * Returns an {@link Optional} containing the current {@link Shift}. Will return {@link
+   * Optional#empty()} if disabled or in between auto and teleop.
+   */
+  public static Optional<Shift> getCurrentShift() {
+    double matchTime = getMatchTime();
+    if (matchTime < 0) return Optional.empty();
+
+    for (Shift shift : Shift.values()) {
+      if (matchTime < shift.endTime) {
+        return Optional.of(shift);
+      }
+    }
+    return Optional.empty();
+  }
+
+  /**
+   * Returns an {@link Optional} containing the current {@link Time} remaining in the current shift.
+   * Will return {@link Optional#empty()} if disabled or in between auto and teleop.
+   */
+  public static Optional<Time> timeRemainingInCurrentShift() {
+    return getCurrentShift().map((shift) -> Seconds.of(shift.endTime - getMatchTime()));
+  }
+
+  /**
+   * Returns an {@link Optional} containing the next {@link Shift}. Will return {@link
+   * Optional#empty()} if disabled or in between auto and teleop.
+   */
+  public static Optional<Shift> getNextShift() {
+    double matchTime = getMatchTime();
+
+    for (Shift shift : Shift.values()) {
+      if (matchTime < shift.startTime) {
+        return Optional.of(shift);
+      }
+    }
+    return Optional.empty();
+  }
+
+  /**
+   * Returns whether the hub is active during the specified {@link Shift} for the specified {@link
+   * Alliance}. Will return {@code false} if disabled or in between auto and teleop.
+   */
+  public static boolean isActive(Alliance alliance, Shift shift) {
+    Optional<Alliance> autoWinner = getAutoWinner();
+    switch (shift.activeType) {
+      case BOTH:
+        return true;
+      case AUTO_WINNER:
+        return autoWinner.isPresent() && autoWinner.get() == alliance;
+      case AUTO_LOSER:
+        return autoWinner.isPresent() && autoWinner.get() != alliance;
+      default:
+        return false;
+    }
+  }
+
+  /**
+   * Returns whether the hub is active during the current {@link Shift} for the specified {@link
+   * Alliance}. Will return {@code false} if disabled or in between auto and teleop.
+   */
+  public static boolean isActive(Alliance alliance) {
+    Optional<Shift> currentShift = getCurrentShift();
+    return currentShift.isPresent() && isActive(alliance, currentShift.get());
+  }
+
+  /**
+   * Returns whether the hub is active during the specified {@link Shift} for the robot's {@link
+   * Alliance}. Will return {@code false} if disabled or in between auto and teleop.
+   */
+  public static boolean isActive(Shift shift) {
+    Optional<Alliance> alliance = DriverStation.getAlliance();
+    return alliance.isPresent() && isActive(alliance.get(), shift);
+  }
+
+  /**
+   * Returns whether the hub is active during the current {@link Shift} for the robot's {@link
+   * Alliance}. Will return {@code false} if disabled or in between auto and teleop.
+   */
+  public static boolean isActive() {
+    Optional<Shift> currentShift = getCurrentShift();
+    Optional<Alliance> alliance = DriverStation.getAlliance();
+    return currentShift.isPresent()
+        && alliance.isPresent()
+        && isActive(alliance.get(), currentShift.get());
+  }
+
+  /**
+   * Returns whether the hub is active for the next {@link Shift} for the specified {@link
+   * Alliance}. Will return {@code false} if disabled or in between auto and teleop.
+   */
+  public static boolean isActiveNext(Alliance alliance) {
+    Optional<Shift> nextShift = getNextShift();
+    return nextShift.isPresent() && isActive(alliance, nextShift.get());
+  }
+
+  /**
+   * Returns whether the hub is active during the specified {@link Shift} for the specified {@link
+   * Alliance}. Will return {@code false} if disabled or in between auto and teleop.
+   */
+  public static boolean isActiveNext() {
+    Optional<Shift> nextShift = getNextShift();
+    Optional<Alliance> alliance = DriverStation.getAlliance();
+    return nextShift.isPresent()
+        && alliance.isPresent()
+        && isActive(alliance.get(), nextShift.get());
+  }
+
+  /**
+   * Returns the {@link Alliance} that won auto as specified by the FMS/Driver Station's game
+   * specific message data. Will return {@link Optional#empty()} if no game message or alliance is
+   * available.
+   */
+  public static Optional<Alliance> getAutoWinner() {
+    String msg = DriverStation.getGameSpecificMessage();
+    char msgChar = msg.length() > 0 ? msg.charAt(0) : ' ';
+    switch (msgChar) {
+      case 'B':
+        return Optional.of(Alliance.Blue);
+      case 'R':
+        return Optional.of(Alliance.Red);
+      default:
+        return Optional.empty();
+    }
+  }
+
+  /**
+   * Counts up from 0 to 160 seconds as match progresses. Returns -1 if not match isn't running or
+   * if in between auto and teleop
+   */
+  public static double getMatchTime() {
+    if (DriverStation.isAutonomous()) {
+      if (DriverStation.getMatchTime() < 0) return DriverStation.getMatchTime();
+      return 20 - DriverStation.getMatchTime();
+    } else if (DriverStation.isTeleop()) {
+      if (DriverStation.getMatchTime() < 0) return DriverStation.getMatchTime();
+      return 160 - DriverStation.getMatchTime();
+    }
+    return -1;
+  }
+
+  /**
+   * Represents an alliance shift.<br>
+   *
+   * <h4>Values:</h4>
+   *
+   * <ul>
+   *   <li>{@link Shift#AUTO} (0-20 sec)
+   *   <li>{@link Shift#TRANSITION} (20-30 sec)
+   *   <li>{@link Shift#SHIFT_1} (30-55 sec)
+   *   <li>{@link Shift#SHIFT_2} (55-80 sec)
+   *   <li>{@link Shift#SHIFT_3} (80-105 sec)
+   *   <li>{@link Shift#SHIFT_4} (105-130 sec)
+   *   <li>{@link Shift#ENDGAME} (130-160 sec)
+   * </ul>
+   */
+  /** Configures the start and end times of shifts and stores ActiveType and */
+  public enum Shift {
+    AUTO(0, 20, ActiveType.BOTH),
+    TRANSITION(20, 30, ActiveType.BOTH),
+    SHIFT_1(30, 55, ActiveType.AUTO_LOSER),
+    SHIFT_2(55, 80, ActiveType.AUTO_WINNER),
+    SHIFT_3(80, 105, ActiveType.AUTO_LOSER),
+    SHIFT_4(105, 130, ActiveType.AUTO_WINNER),
+    ENDGAME(130, 160, ActiveType.BOTH);
+
+    final int startTime;
+    final int endTime;
+    final ActiveType activeType;
+    /** Constructs a shift with the range of time and active type constraints */
+    private Shift(int startTime, int endTime, ActiveType activeType) {
+      this.startTime = startTime;
+      this.endTime = endTime;
+      this.activeType = activeType;
+    }
+  }
+  /** Defines possible states and types for the active hub tracker */
+  private enum ActiveType {
+    BOTH,
+    AUTO_WINNER,
+    AUTO_LOSER
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/Rebuilt.java b/src/main/java/frc/robot/rebuilt/Rebuilt.java
new file mode 100644
index 00000000..73f891e4
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/Rebuilt.java
@@ -0,0 +1,142 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.commands.AutoCommands;
+import frc.robot.rebuilt.commands.ClimbCommands;
+import frc.robot.rebuilt.commands.IndexerCommands;
+import frc.robot.rebuilt.commands.IntakeCommands;
+import frc.robot.rebuilt.commands.LauncherCommands;
+import frc.robot.rebuilt.commands.NamedCommandsReg;
+import frc.robot.rebuilt.commands.TestCommands;
+import frc.robot.rebuilt.subsystems.Climb.Climb;
+import frc.robot.rebuilt.subsystems.DriverDisplay.HubStatus;
+import frc.robot.rebuilt.subsystems.Indexer.Indexer;
+import frc.robot.rebuilt.subsystems.Launcher.FieldRegions;
+import frc.robot.rebuilt.subsystems.Launcher.Launcher;
+import frc.robot.rebuilt.subsystems.intake.Intake;
+import org.frc5010.common.arch.GenericRobot;
+import org.frc5010.common.config.ConfigConstants;
+import org.frc5010.common.drive.GenericDrivetrain;
+import org.frc5010.common.sensors.Controller;
+import org.frc5010.common.utils.OrchestraManager;
+import org.frc5010.common.utils.geometry.AllianceFlipUtil;
+
+/** This is an example robot class. */
+/** Long's correction: Main robot class that initializes subsystems and commands */
+public class Rebuilt extends GenericRobot {
+  public static HubStatus hubStatus = new HubStatus();
+  public static GenericDrivetrain drivetrain;
+  public static Indexer indexer;
+  public static Climb climb;
+  public static Intake intake;
+  public static Launcher launcher;
+  public static LauncherCommands launcherCommands;
+  public static AutoCommands autocommands;
+  public static ClimbCommands climbCommands;
+  public static IntakeCommands intakecommands;
+  public static IndexerCommands indexerCommands;
+  public static TestCommands testCommands;
+  private boolean isButtonsConfigured = false;
+  private boolean isAltButtonsConfigured = false;
+
+  public Rebuilt(String directory) {
+    super(directory);
+    AllianceFlipUtil.configure(FieldConstants.FIELD_WIDTH, FieldConstants.FIELD_LENGTH);
+    /** creating robot subsystems */
+    indexer = new Indexer();
+    // climb = new Climb();
+    intake = new Intake();
+    launcher = new Launcher(subsystems);
+    drivetrain = (GenericDrivetrain) subsystems.get(ConfigConstants.DRIVETRAIN);
+    /** creates command containers */
+    testCommands = new TestCommands(subsystems);
+    climbCommands = new ClimbCommands(subsystems);
+    launcherCommands = new LauncherCommands(subsystems);
+    intakecommands = new IntakeCommands(subsystems);
+    indexerCommands = new IndexerCommands(subsystems);
+    autocommands = new AutoCommands(subsystems);
+    OrchestraManager.loadMusic("raiders");
+  }
+
+  @Override
+  public void disabledInit() {
+    OrchestraManager.play();
+  }
+
+  @Override
+  public void disabledPeriodic() {
+    super.disabledPeriodic();
+    SmartDashboard.putBoolean("Orchestra Playing", OrchestraManager.isPlaying());
+  }
+
+  @Override
+  /** Configures buttons with commands */
+  public void configureButtonBindings(Controller driver, Controller operator) {
+    if (!isButtonsConfigured) {
+      FieldRegions.setupFieldRegions();
+      driver.createYButton().onTrue(Commands.runOnce(() -> drivetrain.toggleFieldOrientedDrive()));
+      drivetrain.configureButtonBindings(driver, operator);
+      climbCommands.configureButtonBindings(driver, operator);
+      launcherCommands.configureButtonBindings(driver, operator);
+      intakecommands.configureButtonBindings(driver, operator);
+      indexerCommands.configureButtonBindings(driver, operator);
+      hubStatus.configureButtonBindings(driver, operator);
+      isButtonsConfigured = true;
+    }
+  }
+
+  @Override
+  public void configureAltButtonBindings(Controller driver, Controller operator) {
+    // Add test mode specific button bindings here
+    if (!isAltButtonsConfigured) {
+      testCommands.configureButtonBindings(driver);
+      isAltButtonsConfigured = true;
+    }
+  }
+
+  @Override
+  /** Assigns default commands for each subsystem */
+  public void setupDefaultCommands(Controller driver, Controller operator) {
+    OrchestraManager.stop();
+    // This is part of auto init, so a good place to run this
+    FieldRegions.setupFieldRegions();
+    drivetrain.setDefaultCommand(drivetrain.createDefaultCommand(driver));
+    launcherCommands.setDefaultCommands();
+    indexerCommands.setupDefaultCommands();
+    intakecommands.setupDefaultCommands();
+  }
+
+  @Override
+  public void initAutoCommands() {
+    NamedCommandsReg.createNamedCommands();
+    drivetrain.setAutoBuilder();
+  }
+
+  // @Override
+  // public Command getAutonomousCommand() {
+  //   if (DriverStation.isFMSAttached()) {
+  //     intake.setHopperPosition(Constants.Intake.HOPPER_RETRACTED_ANGLE);
+  //   }
+  //   return super.getAutonomousCommand();
+  // }
+
+  @Override
+  public Command generateAutoCommand(Command autoCommand) {
+    return drivetrain.generateAutoCommand(autoCommand);
+  }
+
+  @Override
+  /** Creates and registers available auto comands */
+  public void buildAutoCommands() {
+    super.buildAutoCommands();
+    selectableCommand.addOption("Do Nothing", Commands.none());
+    drivetrain.addAutoCommands(selectableCommand);
+    autocommands.configureCharacterizationCommands(selectableCommand);
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/AutoCommands.java b/src/main/java/frc/robot/rebuilt/commands/AutoCommands.java
new file mode 100644
index 00000000..daeb02f1
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/AutoCommands.java
@@ -0,0 +1,69 @@
+package frc.robot.rebuilt.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.Constants;
+import frc.robot.rebuilt.subsystems.Launcher.Launcher;
+import frc.robot.rebuilt.subsystems.intake.Intake;
+import java.util.Map;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.drive.GenericDrivetrain;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+
+public class AutoCommands {
+
+  private Map<String, GenericSubsystem> subsystems;
+
+  public AutoCommands(Map<String, GenericSubsystem> subsystems) {
+    this.subsystems = subsystems;
+  }
+
+  public void configureNamedCommands() {}
+
+  public void configureCharacterizationCommands(LoggedDashboardChooser<Command> selectableCommand) {
+    selectableCommand.addOption(
+        "PRO: Intake Hopper Characterization",
+        ((Intake) subsystems.get(Constants.INTAKE)).getHopperCharacterizationCommand());
+    selectableCommand.addOption(
+        "PRO: Launcher Hood Characterization",
+        ((Launcher) subsystems.get(Constants.LAUNCHER)).getHoodCharacterizationCommand());
+    selectableCommand.addOption(
+        "PRO: Launcher Turret Characterization",
+        ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretCharacterizationCommand());
+    selectableCommand.addOption(
+        "TUNE: Shot Lookup Table Tuning",
+        ShotCalibrationCommand.createWithFeed(
+            (Launcher) subsystems.get(Constants.LAUNCHER),
+            (GenericDrivetrain)
+                subsystems.get(org.frc5010.common.config.ConfigConstants.DRIVETRAIN),
+            2.0,
+            0.5));
+    selectableCommand.addOption(
+        "PRO: Turret Quasistatic (kS, kV)",
+        ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretQuasistaticCommand());
+    selectableCommand.addOption(
+        "PRO: Turret Dynamic (kA)",
+        ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretDynamicCommand());
+    selectableCommand.addOption(
+        "TUNE: Turret kS Map Generation",
+        ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretKsMapCommand());
+    selectableCommand.addOption(
+        "TUNE: Turret Tracking Sinusoidal Tuning",
+        ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretTrackingTuneCommand());
+
+    selectableCommand.addOption(
+        "TUNE: Turret Seeking Tuning",
+        ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretSeekingTuneCommand());
+  }
+
+  public void configureBasicAutoCommands(LoggedDashboardChooser<Command> selectableCommand) {
+    selectableCommand.addOption(
+        "Shoot Preload Only",
+        Commands.sequence(
+            IntakeCommands.shouldIntaking(),
+            Commands.waitSeconds(2),
+            LauncherCommands.shouldPrepCommand(),
+            Commands.waitSeconds(2),
+            IndexerCommands.shouldForceCommand()));
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/CharacterizationSample.java b/src/main/java/frc/robot/rebuilt/commands/CharacterizationSample.java
new file mode 100644
index 00000000..ea9d9441
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/CharacterizationSample.java
@@ -0,0 +1,17 @@
+package frc.robot.rebuilt.commands;
+
+/**
+ * A single high-frequency characterization sample collected at ~250 Hz.
+ *
+ * @param timestampSeconds FPGA timestamp in seconds when the sample was taken
+ * @param positionRot turret position in mechanism rotations
+ * @param velocityRotPerSec turret velocity in mechanism rot/s
+ * @param accelerationRotPerSecSq turret acceleration in mechanism rot/s^2 (from TalonFX signal)
+ * @param currentAmps measured torque current in Amps (from TalonFX signal)
+ */
+public record CharacterizationSample(
+    double timestampSeconds,
+    double positionRot,
+    double velocityRotPerSec,
+    double accelerationRotPerSecSq,
+    double currentAmps) {}
diff --git a/src/main/java/frc/robot/rebuilt/commands/ClimbCommands.java b/src/main/java/frc/robot/rebuilt/commands/ClimbCommands.java
new file mode 100644
index 00000000..daa5f24d
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/ClimbCommands.java
@@ -0,0 +1,186 @@
+package frc.robot.rebuilt.commands;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.Constants;
+import frc.robot.rebuilt.Constants.ClimbConstants;
+import frc.robot.rebuilt.subsystems.Climb.Climb;
+import java.util.Map;
+import java.util.function.DoubleSupplier;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.arch.StateMachine;
+import org.frc5010.common.arch.StateMachine.State;
+import org.frc5010.common.sensors.Controller;
+
+public class ClimbCommands {
+
+  private Map<String, GenericSubsystem> subsystems;
+  private StateMachine stateMachine;
+  private State idleState;
+  private State elevateState;
+  private State descendState;
+  private State liftedState;
+  private State loweredState;
+  private State disabledState;
+  private State manualState;
+  /** defines possible states for the climb */
+  public static enum ClimbState {
+    IDLE,
+    ELEVATE,
+    LIFTED,
+    DESCEND,
+    LOWERED,
+    DISABLED,
+    MANUAL
+  }
+
+  private static Climb climb;
+  private DoubleSupplier getOpleftY;
+
+  public ClimbCommands(Map<String, GenericSubsystem> systems) {
+    this.subsystems = systems;
+
+    // Create a simple state machine for climb and set it as the default command for the Climb
+    climb = (Climb) subsystems.get(Constants.CLIMB);
+    if (null == climb) {
+      return;
+    }
+    stateMachine = new StateMachine("ClimbStateMachine");
+    // a simple idle state; transitions will be added in configureButtonBindings
+    idleState =
+        stateMachine.addState(
+            "idle",
+            Commands.runOnce(() -> climb.runClimb(0))
+                .alongWith(Commands.runOnce(() -> climb.setCurrentState(ClimbState.IDLE))));
+    loweredState =
+        stateMachine.addState(
+            "lowered", Commands.runOnce(() -> climb.setCurrentState(ClimbState.LOWERED)));
+    liftedState =
+        stateMachine.addState(
+            "lifted", Commands.runOnce(() -> climb.setCurrentState(ClimbState.LIFTED)));
+    disabledState =
+        stateMachine.addState(
+            "disabled", Commands.runOnce(() -> climb.setCurrentState(ClimbState.DISABLED)));
+    manualState =
+        stateMachine.addState(
+            "manual",
+            Commands.runOnce(() -> climb.setCurrentState(ClimbState.MANUAL))
+                .alongWith(
+                    Commands.run(
+                        () ->
+                            climb.setDefaultCommand(
+                                Commands.run(
+                                    () -> {
+                                      climb.runClimb(getOpleftY.getAsDouble());
+                                    },
+                                    climb)))));
+
+    // states that actually run the climber
+    if (climb != null) {
+      elevateState =
+          stateMachine.addState(
+              "elevate",
+              climb
+                  .climberCommand(Meters.of(.5))
+                  .alongWith(Commands.runOnce(() -> climb.setCurrentState(ClimbState.ELEVATE))));
+      descendState =
+          stateMachine.addState(
+              "lower",
+              climb
+                  .climberCommand(Meters.of(0))
+                  .alongWith(Commands.runOnce(() -> climb.setCurrentState(ClimbState.DESCEND))));
+    } else {
+      // fallback states if climb isn't available
+      elevateState = stateMachine.addState("elevate", Commands.idle());
+      descendState = stateMachine.addState("lower", Commands.idle());
+    }
+
+    // Set DISABLED as the default initial state
+    stateMachine.setInitialState(disabledState);
+
+    if (climb != null) {
+      stateMachine.addRequirements(climb);
+      climb.setDefaultCommand(stateMachine);
+    }
+  }
+
+  public static Command shouldElevateCommand() {
+    return Commands.runOnce(() -> climb.setRequestedState(ClimbState.ELEVATE));
+  }
+
+  public static Command shouldStopCommand() {
+    return Commands.runOnce(() -> climb.setRequestedState(ClimbState.IDLE));
+  }
+
+  public static Command shouldDescendCommand() {
+    return Commands.runOnce(() -> climb.setRequestedState(ClimbState.DESCEND));
+  }
+
+  // New: command to enable the climb (requests IDLE)
+  public static Command shouldEnableCommand() {
+    return Commands.runOnce(() -> climb.setRequestedState(ClimbState.IDLE));
+  }
+
+  public void configureButtonBindings(Controller driver, Controller operator) {
+    if (null == climb) {
+      return;
+    }
+
+    // Bind the "enable climb" button to transition DISABLED -> IDLE when pressed.
+    // Change createStartButton() to whatever button you prefer on your controller.
+    operator.createStartButton().onTrue(shouldEnableCommand());
+
+    // Driver POV-Up enables the climb (requests IDLE)
+    driver.createUpPovButton().onTrue(shouldEnableCommand());
+
+    // disabled -> idle when the enable command is requested
+    disabledState.switchTo(idleState).when(() -> climb.isRequested(ClimbState.IDLE));
+
+    configCommonStates(operator);
+  }
+
+  public void configureAltButtonBindings(Controller driver, Controller operator) {
+    stateMachine.setInitialState(idleState);
+
+    operator.createXButton().onTrue(shouldElevateCommand()).onFalse(shouldStopCommand());
+    operator.createYButton().onTrue(shouldDescendCommand()).onFalse(shouldStopCommand());
+    // lowered -> elevate when requested
+
+    stateMachine.setInitialState(idleState);
+
+    configCommonStates(operator);
+  }
+
+  private void configCommonStates(Controller operator) {
+
+    loweredState.switchTo(elevateState).when(() -> climb.isRequested(ClimbState.ELEVATE));
+    // descend -> lowered when height is Zero
+    descendState.switchTo(loweredState).when(() -> climb.getHeight().isEquivalent(Meters.of(0)));
+    // elevate -> Lifted when height is =to Target
+    elevateState
+        .switchTo(liftedState)
+        .when(() -> climb.getHeight().isEquivalent(ClimbConstants.MAX));
+    // lifted -> descend when asked to descend
+    liftedState.switchTo(descendState).when(() -> climb.isRequested(ClimbState.DESCEND));
+    // elevate -> descend when asked to descend
+    elevateState.switchTo(descendState).when(() -> climb.isRequested(ClimbState.DESCEND));
+    // descend -> elevating when asked to elevate
+    descendState.switchTo(elevateState).when(() -> climb.isRequested(ClimbState.ELEVATE));
+    // elevate -> idle when stopped
+    elevateState.switchTo(idleState).when(() -> climb.isRequested(ClimbState.IDLE));
+    // descend -> idle when stopped
+    descendState.switchTo(idleState).when(() -> climb.isRequested(ClimbState.IDLE));
+    // idle -> elevate when requested
+    idleState.switchTo(elevateState).when(() -> climb.isRequested(ClimbState.ELEVATE));
+    // idle -> descend when requested
+    idleState.switchTo(descendState).when(() -> climb.isRequested(ClimbState.DESCEND));
+    // idle- > manual
+    idleState.switchTo(manualState).when(() -> operator.getLeftYAxis() != 0);
+    // manual to switch
+    manualState.switchTo(idleState).when(() -> operator.getLeftYAxis() == 0);
+
+    getOpleftY = () -> operator.getLeftYAxis();
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/IndexerCommands.java b/src/main/java/frc/robot/rebuilt/commands/IndexerCommands.java
new file mode 100644
index 00000000..7a53226f
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/IndexerCommands.java
@@ -0,0 +1,183 @@
+package frc.robot.rebuilt.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.rebuilt.Constants;
+import frc.robot.rebuilt.subsystems.Indexer.Indexer;
+import frc.robot.rebuilt.subsystems.Launcher.Launcher;
+import java.util.Map;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.config.ConfigConstants;
+import org.frc5010.common.sensors.Controller;
+import org.frc5010.common.subsystems.LEDStrip;
+
+public class IndexerCommands {
+  /** declares variables that will later hold state objects */
+  private Map<String, GenericSubsystem> subsystems;
+
+  private static Indexer indexer;
+  private static Launcher launcher;
+
+  /** defines possible states of the indexer */
+  public static enum IndexerState {
+    IDLE,
+    CHURN,
+    HARD_CHURN,
+    FORCE,
+    FEED
+  }
+
+  /** Stores the subsystem map and retrieves the indexer instance */
+  public IndexerCommands(Map<String, GenericSubsystem> systems) {
+    this.subsystems = systems;
+    IndexerCommands.indexer = (Indexer) subsystems.get(Constants.INDEXER);
+    IndexerCommands.launcher = (Launcher) subsystems.get(Constants.LAUNCHER);
+    configureTriggerStates();
+  }
+
+  public void configureButtonBindings(Controller driver, Controller operator) {
+    // driver.createLeftBumper().onTrue(toggleForceFeed());
+    // driver.createLeftBumper().whileTrue(shouldForceCommand()).onFalse(shouldChurnCommand());
+    driver.createLeftBumper().whileTrue(shouldHardChurnCommand()).onFalse(shouldChurnCommand());
+    operator
+        .createLeftBumper()
+        .whileTrue(
+            Commands.either(
+                shouldForceCommand(), shouldChurnCommand(), () -> launcher.isOKToFire()))
+        .onFalse(shouldChurnCommand());
+  }
+
+  private void configureTriggerStates() {
+    // Map requested states to their commands and wire triggers in a compact loop.
+    // CHURN is handled separately below so it can be gated on flywheel readiness.
+    java.util.Map<IndexerState, Command> stateToCommand =
+        java.util.Map.of(
+            IndexerState.FEED, feedStateCommand(),
+            IndexerState.FORCE, forceStateCommand(),
+            IndexerState.IDLE, idleStateCommand(),
+            IndexerState.HARD_CHURN, hardChurnStateCommand());
+
+    stateToCommand.forEach(
+        (state, cmd) -> new Trigger(() -> indexer.isRequested(state)).onTrue(cmd));
+
+    // CHURN: the request can be set at any time, but the indexer only physically
+    // starts churning once LauncherCommands.isFlywheelReadyForChurn() is satisfied.
+    new Trigger(() -> indexer.isRequested(IndexerState.CHURN)).onTrue(churnStateCommand());
+  }
+
+  public void setupDefaultCommands() {}
+
+  /** defines command behavio for the force state stops the indexer and runs the transfer at 50% */
+  public static Command forceStateCommand() {
+    return Commands.runOnce(
+        () -> {
+          indexer.setCurrentState(IndexerState.FORCE);
+          indexer.runSpindexer(Constants.Indexer.SPINDEXER_SPEED);
+          indexer.runTransferFront(Constants.Indexer.TRANSFER_SPEED);
+          //          indexer.runTransferBack(0.50);
+        },
+        indexer);
+  }
+  /** defines command behavior for the churn state stops the indexer and runs the transfer at 25% */
+  private static Command churnStateCommand() {
+    return Commands.runOnce(
+            () -> {
+              indexer.setCurrentState(IndexerState.CHURN);
+              indexer.runSpindexer(-0.1);
+              indexer.runTransferFront(Constants.Indexer.TRANSFER_CHURN);
+            },
+            indexer)
+        .andThen(Commands.waitSeconds(1.0))
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  indexer.runSpindexer(0.0);
+                  indexer.runTransferFront(0);
+                }));
+  }
+
+  private static Command hardChurnStateCommand() {
+    return Commands.runOnce(
+        () -> {
+          indexer.setCurrentState(IndexerState.HARD_CHURN);
+          indexer.runSpindexer(-0.5);
+          indexer.runTransferFront(Constants.Indexer.TRANSFER_CHURN);
+        },
+        indexer);
+  }
+
+  /**
+   * defines command behavior for the idle state stops all motors and sets the LED patters to
+   * rainbow
+   */
+  private static Command idleStateCommand() {
+    return Commands.runOnce(
+        () -> {
+          indexer.setCurrentState(IndexerState.IDLE);
+          indexer.runSpindexer(0);
+          indexer.runTransferFront(0);
+          //          indexer.runTransferBack(0);
+          LEDStrip.changeSegmentPattern(ConfigConstants.ALL_LEDS, LEDStrip.getRainbowPattern(0));
+        },
+        indexer);
+  }
+
+  // run feed command when Launcher State is idle and Operator Right Bumper is
+  // pressed
+  private static Command feedStateCommand() {
+    return Commands.parallel(
+        Commands.runOnce(
+            () -> {
+              indexer.setCurrentState(IndexerState.FEED);
+              indexer.runSpindexer(Constants.Indexer.SPINDEXER_SPEED);
+              indexer.runTransferFront(Constants.Indexer.TRANSFER_SPEED);
+              //              indexer.runTransferBack(1);
+              LEDStrip.changeSegmentPattern(
+                  ConfigConstants.ALL_LEDS, LEDStrip.getRainbowPattern(25));
+            },
+            indexer));
+  }
+  /** Requests the indexer to enter the idle state */
+  public static Command shouldIdleCommand() {
+    return Commands.runOnce(() -> indexer.setRequestedState(IndexerState.IDLE));
+  }
+  /** Requests the indexer to enter the churn state */
+  public static Command shouldChurnCommand() {
+    return Commands.runOnce(() -> indexer.setRequestedState(IndexerState.CHURN));
+  }
+
+  public static Command churnAuto() {
+    return Commands.run(
+        () -> {
+          indexer.runSpindexer(-0.1);
+        });
+  }
+
+  public static Command shouldHardChurnCommand() {
+    return Commands.runOnce(() -> indexer.setRequestedState(IndexerState.HARD_CHURN));
+  }
+  /** Requests the indexer to enter the feed state */
+  public static Command shouldFeedCommand() {
+    return Commands.runOnce(() -> indexer.setRequestedState(IndexerState.FEED));
+  }
+
+  public static Command shouldForceCommand() {
+    return Commands.runOnce(
+        () -> {
+          indexer.setRequestedState(IndexerState.FORCE);
+        });
+  }
+
+  public static Command toggleForceFeed() {
+    return Commands.runOnce(
+        () -> {
+          if (indexer.isRequested(IndexerState.FEED)) {
+            indexer.setRequestedState(IndexerState.CHURN);
+
+          } else {
+            indexer.setRequestedState(IndexerState.FEED);
+          }
+        });
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/IntakeCommands.java b/src/main/java/frc/robot/rebuilt/commands/IntakeCommands.java
new file mode 100644
index 00000000..cced35fe
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/IntakeCommands.java
@@ -0,0 +1,269 @@
+package frc.robot.rebuilt.commands;
+
+import static edu.wpi.first.units.Units.Degrees;
+
+import edu.wpi.first.wpilibj.RobotState;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.rebuilt.Constants;
+import frc.robot.rebuilt.subsystems.intake.Intake;
+import java.util.Map;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.arch.StateMachine;
+import org.frc5010.common.sensors.Controller;
+
+public class IntakeCommands {
+  static Intake intake;
+  static boolean positionFound = false;
+  Map<String, GenericSubsystem> subsystems;
+  StateMachine intakeStateMachine = new StateMachine("IntakeStateMachine");
+
+  DoubleSupplier intakeSpeedSupplier =
+      () -> Constants.Intake.INTAKE_IN; // Default speed, can be overridden by triggers
+  Supplier<DoubleSupplier> intakeSpeed = () -> intakeSpeedSupplier;
+
+  public static enum IntakeState {
+    UNKNOWN,
+    RETRACTED,
+    RETRACTING,
+    DEPLOYING,
+    INTAKING,
+    DEPLOYED,
+    ANGLED;
+  }
+
+  public IntakeCommands(Map<String, GenericSubsystem> subsystems) {
+    this.subsystems = subsystems;
+
+    intake = (Intake) subsystems.get(Constants.INTAKE);
+
+    setupTriggerStates();
+  }
+
+  public void setupDefaultCommands() {}
+
+  private void setupTriggerStates() {
+    // Map requested states to their commands and wire triggers in a compact loop.
+    // CHURN is handled separately below so it can be gated on flywheel readiness.
+    java.util.Map<IntakeState, Command> stateToCommand =
+        java.util.Map.of(
+            IntakeState.UNKNOWN, unknownStateCommand(),
+            IntakeState.RETRACTED, retractedCommand(),
+            IntakeState.DEPLOYED, deployedCommand());
+
+    stateToCommand.forEach(
+        (state, cmd) -> new Trigger(() -> intake.isRequested(state)).onTrue(cmd));
+
+    // Not moving trigger senses if the hopper has hit the bumper hard stop for 0.5 sec
+    Trigger hopperNotMovingAndLowish =
+        new Trigger(() -> intake.isHopperStalling()).debounce(Constants.Intake.HOPPER_STALL_TIME);
+
+    /** Trigger the deploying command */
+    new Trigger(
+            () ->
+                intake.isRequested(IntakeState.INTAKING) && !intake.isCurrent(IntakeState.INTAKING))
+        .onTrue(deployingCommand().until(() -> intake.isCurrent(IntakeState.INTAKING)));
+
+    /** Trigger the intaking command */
+    new Trigger(
+            () ->
+                intake.isRequested(IntakeState.INTAKING)
+                    && intake.isCurrent(IntakeState.DEPLOYING)
+                    && (intake.isDeployed() || hopperNotMovingAndLowish.getAsBoolean()))
+        .onTrue(intakingCommand(intakeSpeed));
+
+    /** Trigger the retracting command */
+    new Trigger(
+            () ->
+                intake.isRequested(IntakeState.RETRACTING)
+                    && !intake.isCurrent(IntakeState.RETRACTED))
+        .onTrue(retractingCommand().until(() -> intake.isCurrent(IntakeState.RETRACTED)));
+
+    /** Trigger the retracted command */
+    new Trigger(() -> intake.isCurrent(IntakeState.RETRACTING) && (intake.isRetracted()))
+        .onTrue(shouldRetracted());
+
+    /** Trigger the angled command */
+    new Trigger(
+            () -> intake.isRequested(IntakeState.ANGLED) && !intake.isCurrent(IntakeState.ANGLED))
+        .onTrue(angledCommand());
+  }
+
+  public void configureButtonBindings(Controller controller, Controller operator) {
+    controller.setRightTrigger(
+        controller.createRightTrigger().limit(Constants.Intake.INTAKE_MAX_IN));
+    Trigger rightTrigger =
+        new Trigger(() -> controller.getRightTrigger() > Constants.Intake.INTAKE_DEADZONE);
+    controller.setLeftTrigger(
+        controller
+            .createLeftTrigger()
+            .limit(Constants.Intake.INTAKE_MAX_IN)); // Axis are positive only hence IN
+    Trigger leftTrigger =
+        new Trigger(() -> controller.getLeftTrigger() > Constants.Intake.INTAKE_DEADZONE);
+
+    rightTrigger.onTrue(shouldIntaking());
+    leftTrigger.onTrue(shouldIntaking());
+
+    controller.createRightBumper().onTrue(shouldRetracting());
+    controller.createStartButton().onTrue(Commands.run(() -> intake.setHopperRetracted()));
+    controller.createBackButton().onTrue(Commands.run(() -> intake.setHopperDeployed()));
+
+    operator.createDownPovButton().onTrue(operatorHopperDownCommand());
+    controller.createXButton().onTrue(operatorHopperDownCommand());
+    operator.createRightBumper().onTrue(shouldAngled()).onFalse(shouldIntaking());
+
+    intakeSpeedSupplier =
+        () -> {
+          double rightTriggerSpeed = controller.getRightTrigger();
+          double leftTriggerSpeed = controller.getLeftTrigger();
+          double speed = Constants.Intake.INTAKE_IN; // Default speed if neither trigger is pressed
+          if (rightTriggerSpeed > Constants.Intake.INTAKE_DEADZONE
+              || leftTriggerSpeed > Constants.Intake.INTAKE_DEADZONE) {
+            speed =
+                rightTriggerSpeed
+                    - leftTriggerSpeed; // Positive for intaking, negative for outtaking
+          }
+          if (RobotState.isAutonomous()) {
+            speed = Constants.Intake.INTAKE_AUTO; // Intake in speed in auto
+          }
+          return speed;
+        };
+  }
+
+  public static Command intakingCommand(Supplier<DoubleSupplier> speed) {
+    return Commands.runOnce(
+            () -> {
+              intake.setCurrentState(IntakeState.INTAKING);
+              positionFound = true;
+              intake.setHopperPosition(Degrees.of(0));
+            },
+            intake)
+        .andThen(
+            Commands.run(
+                () -> {
+                  double runSpeed = speed.get().getAsDouble();
+
+                  if (intake.getHopperAngle().gt(Degrees.of(5))
+                      || runSpeed > Constants.Intake.INTAKE_IN
+                      || RobotState.isAutonomous()) {
+                    intake.runHopper(-0.15);
+                  } else {
+                    intake.setDesiredHopperAngle(Constants.Intake.HOPPER_DEPLOYED_ANGLE);
+                  }
+                  intake.runSpintake(runSpeed);
+                },
+                intake));
+  }
+
+  public static Command waitUntilIntaking() {
+    return Commands.idle().until(() -> intake.isCurrent(IntakeState.INTAKING));
+  }
+
+  public static Command deployingCommand() {
+    return Commands.runOnce(
+            () -> {
+              intake.setCurrentState(IntakeState.DEPLOYING);
+              intake.setHopperPosition(Constants.Intake.HOPPER_RETRACTED_ANGLE);
+            },
+            intake)
+        .andThen(
+            intake
+                .setDesiredHopperAngle(Constants.Intake.HOPPER_DEPLOYED_ANGLE)
+                .until(() -> intake.isHopperAtGoal())
+                .andThen(
+                    Commands.runOnce(
+                        () -> {
+                          intake.runHopper(-0.1);
+                        })))
+        .alongWith(
+            Commands.run(
+                () -> {
+                  if (intake.getHopperAngle().lt(Degrees.of(60))) {
+                    intake.runSpintake(Constants.Intake.INTAKE_IN);
+                  }
+                }));
+  }
+
+  public static Command deployedCommand() {
+    return Commands.runOnce(
+        () -> {
+          intake.setCurrentState(IntakeState.DEPLOYED);
+          intake.runHopper(0);
+        },
+        intake);
+  }
+
+  public static Command angledCommand() {
+    return Commands.runOnce(
+            () -> {
+              intake.setCurrentState(IntakeState.ANGLED);
+            },
+            intake)
+        .andThen(
+            intake
+                .setDesiredHopperAngle(Constants.Intake.HOPPER_ANGLED)
+                .until(() -> intake.isHopperMoving())
+                .andThen(
+                    Commands.run(
+                        () ->
+                            intake.runSpintakes(
+                                Constants.Intake.INTAKE_IN * 0.5, Constants.Intake.INTAKE_CHURN),
+                        intake)));
+  }
+
+  public static Command retractingCommand() {
+    return Commands.runOnce(
+            () -> {
+              intake.setCurrentState(IntakeState.RETRACTING);
+            },
+            intake)
+        .andThen(() -> intake.runSpintake(0), intake)
+        .andThen(
+            intake
+                .setDesiredHopperAngle(Constants.Intake.HOPPER_RETRACTED_ANGLE)
+                .until(() -> intake.isHopperMoving()));
+  }
+
+  public static Command retractedCommand() {
+    return Commands.runOnce(() -> intake.setCurrentState(IntakeState.RETRACTED))
+        .andThen(() -> intake.runSpintake(0), intake)
+        .andThen(() -> intake.runHopper(0), intake);
+  }
+
+  public static Command unknownStateCommand() {
+    return Commands.runOnce(
+            () -> {
+              intake.setCurrentState(IntakeState.UNKNOWN);
+            })
+        .andThen(Commands.runOnce(() -> intake.runHopper(0), intake))
+        .andThen(Commands.runOnce(() -> intake.runSpintake(0), intake));
+  }
+
+  public Command operatorHopperDownCommand() {
+    return Commands.run(
+            () -> {
+              intake.runHopper(-.2);
+            })
+        .until(() -> intake.isHopperStalling())
+        .andThen(intakingCommand(intakeSpeed));
+  }
+
+  public static Command shouldIntaking() {
+    return Commands.runOnce(() -> intake.setRequestedState(IntakeState.INTAKING));
+  }
+
+  public static Command shouldRetracting() {
+    return Commands.runOnce(() -> intake.setRequestedState(IntakeState.RETRACTING));
+  }
+
+  public static Command shouldRetracted() {
+    return Commands.runOnce(() -> intake.setRequestedState(IntakeState.RETRACTED));
+  }
+
+  public static Command shouldAngled() {
+    return Commands.runOnce(() -> intake.setRequestedState(IntakeState.ANGLED));
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/LauncherCommands.java b/src/main/java/frc/robot/rebuilt/commands/LauncherCommands.java
new file mode 100644
index 00000000..5282f7a4
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/LauncherCommands.java
@@ -0,0 +1,455 @@
+package frc.robot.rebuilt.commands;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.Radians;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.rebuilt.Constants;
+import frc.robot.rebuilt.FieldConstants;
+import frc.robot.rebuilt.commands.IntakeCommands.IntakeState;
+import frc.robot.rebuilt.subsystems.Launcher.Launcher;
+import frc.robot.rebuilt.subsystems.Launcher.ShotCalculator;
+import frc.robot.rebuilt.subsystems.Launcher.ShotCalculator.ShootingParameters;
+import frc.robot.rebuilt.subsystems.intake.Intake;
+import java.util.Map;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.arch.StateMachine;
+import org.frc5010.common.arch.StateMachine.State;
+import org.frc5010.common.config.ConfigConstants;
+import org.frc5010.common.drive.GenericDrivetrain;
+import org.frc5010.common.sensors.Controller;
+import org.frc5010.common.subsystems.LEDStrip;
+import org.frc5010.common.utils.geometry.AllianceFlipUtil;
+import org.frc5010.common.vision.AprilTags;
+
+/** defines commands and state launcher logic for the launcher */
+public class LauncherCommands {
+
+  private StateMachine stateMachine;
+  private State idleState;
+  private State lowState;
+  private State prepState;
+  private State presetState;
+  private State hammerTimeState;
+  private State autoHammerTimeState;
+  private State escapeHammerTimeState;
+  private static Launcher launcher;
+  private static Intake intake;
+  private static GenericDrivetrain drivetrain;
+  private Map<String, GenericSubsystem> subsystems;
+  private static Translation2d hubTarget = FieldConstants.Hub.topCenterPoint.toTranslation2d();
+  private static Translation2d allianceSideLeft = FieldConstants.Tower.leftUpright;
+  private static Translation2d allianceSideRight = FieldConstants.Tower.rightUpright;
+  private static Translation2d intakeToCenterTranslation =
+      new Translation2d(Inches.of(25), Inches.of(0));
+  private static Transform2d intakeToCenter =
+      new Transform2d(intakeToCenterTranslation, Rotation2d.fromDegrees(180));
+  private static Translation2d rearToCenterTranslation =
+      new Translation2d(Inches.of(13.5), Inches.of(0));
+  private static Transform2d rearToCenter =
+      new Transform2d(rearToCenterTranslation, Rotation2d.fromDegrees(0));
+
+  // Stored preset targets — written once when a preset command is activated
+  private static Angle presetHoodAngle = Constants.Launcher.LOW_HOOD_ANGLE;
+  private static Angle presetTurretAngle = Constants.Launcher.TURRET_FORWARD;
+  private static AngularVelocity presetFlywheelSpeed = RPM.of(0);
+
+  /**
+   * When true, the indexer will only churn once the flywheel has reached its goal speed. Set to
+   * false to allow churning at any time regardless of flywheel speed.
+   */
+  public static boolean requireFlywheelAtGoalForChurn = true;
+
+  public static Translation2d getRobotToTarget(Translation2d target) {
+    return target.minus(drivetrain.getPoseEstimator().getCurrentPose().getTranslation());
+  }
+  // public static Angle getHoodAngle(Distance toTarget) {} Placeholder for now
+  /** declares possible states for the launcher */
+  public static enum LauncherState {
+    IDLE,
+    LOW_SPEED,
+    PREP,
+    HAMMERTIME,
+    AUTO_HAMMERTIME,
+    ESCAPE_HAMMERTIME,
+    PRESET;
+
+    @Override
+    public String toString() {
+      return this.name();
+    }
+  }
+  /** initializes the launcher state machine and adds states */
+  public LauncherCommands(Map<String, GenericSubsystem> subsystems) {
+    this.subsystems = subsystems;
+    launcher = (Launcher) subsystems.get(Constants.LAUNCHER);
+    intake = (Intake) subsystems.get(Constants.INTAKE);
+    launcher.setCurrentState(LauncherState.HAMMERTIME);
+    launcher.setRequestedState(LauncherState.HAMMERTIME);
+
+    drivetrain = (GenericDrivetrain) this.subsystems.get(ConfigConstants.DRIVETRAIN);
+    configureStateMachine();
+  }
+  /** sets the state machine as the default command of the launcher */
+  public void setDefaultCommands() {
+    if (launcher != null) {
+      stateMachine.addRequirements(launcher);
+      launcher.setDefaultCommand(stateMachine);
+    }
+  }
+
+  public void configureStateMachine() {
+    stateMachine = new StateMachine("LauncherStateMachine");
+    presetState = stateMachine.addState("PRESET-SHOOT", presetStateCommand());
+    idleState = stateMachine.addState("IDLE", idleStateCommand());
+    lowState = stateMachine.addState("LOW-SPEED", lowStateCommand());
+    prepState = stateMachine.addState("PREP-SHOOT", prepStateCommand());
+    hammerTimeState = stateMachine.addState("HAMMER-TIME", hammerTimeStateCommand());
+    autoHammerTimeState = stateMachine.addState("AUTO-HAMMER-TIME", autoHammerTimeStateCommand());
+    escapeHammerTimeState =
+        stateMachine.addState("ESCAPE-HAMMER-TIME", escapeHammerTimeStateCommand());
+    stateMachine.setInitialState(hammerTimeState);
+    idleState.switchTo(lowState).when(() -> launcher.isRequested(LauncherState.LOW_SPEED));
+    idleState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP));
+    idleState.switchTo(presetState).when(() -> launcher.isRequested(LauncherState.PRESET));
+    idleState.switchTo(hammerTimeState).when(() -> launcher.isRequested(LauncherState.HAMMERTIME));
+    idleState
+        .switchTo(autoHammerTimeState)
+        .when(() -> launcher.isRequested(LauncherState.AUTO_HAMMERTIME));
+
+    lowState.switchTo(idleState).when(() -> launcher.isRequested(LauncherState.IDLE));
+    lowState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP));
+    lowState.switchTo(presetState).when(() -> launcher.isRequested(LauncherState.PRESET));
+    lowState.switchTo(hammerTimeState).when(() -> launcher.isRequested(LauncherState.HAMMERTIME));
+    lowState
+        .switchTo(autoHammerTimeState)
+        .when(() -> launcher.isRequested(LauncherState.AUTO_HAMMERTIME));
+
+    prepState.switchTo(lowState).when(() -> launcher.isRequested(LauncherState.LOW_SPEED));
+    prepState.switchTo(idleState).when(() -> launcher.isRequested(LauncherState.IDLE));
+    prepState.switchTo(presetState).when(() -> launcher.isRequested(LauncherState.PRESET));
+    prepState.switchTo(hammerTimeState).when(() -> launcher.isRequested(LauncherState.HAMMERTIME));
+    prepState
+        .switchTo(autoHammerTimeState)
+        .when(() -> launcher.isRequested(LauncherState.AUTO_HAMMERTIME));
+
+    presetState.switchTo(idleState).when(() -> launcher.isRequested(LauncherState.IDLE));
+    presetState.switchTo(lowState).when(() -> launcher.isRequested(LauncherState.LOW_SPEED));
+    presetState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP));
+    presetState
+        .switchTo(hammerTimeState)
+        .when(() -> launcher.isRequested(LauncherState.HAMMERTIME));
+    presetState
+        .switchTo(autoHammerTimeState)
+        .when(() -> launcher.isRequested(LauncherState.AUTO_HAMMERTIME));
+
+    autoHammerTimeState
+        .switchTo(escapeHammerTimeState)
+        .when(() -> launcher.isRequested(LauncherState.ESCAPE_HAMMERTIME));
+
+    escapeHammerTimeState.switchTo(idleState).when(() -> launcher.isRequested(LauncherState.IDLE));
+    escapeHammerTimeState
+        .switchTo(lowState)
+        .when(() -> launcher.isRequested(LauncherState.LOW_SPEED));
+    escapeHammerTimeState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP));
+
+    // Hammer Time is a special case since it's a toggle state
+    hammerTimeState.switchTo(lowState).when(() -> launcher.isRequested(LauncherState.LOW_SPEED));
+    hammerTimeState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP));
+    hammerTimeState.switchTo(presetState).when(() -> launcher.isRequested(LauncherState.PRESET));
+
+    Trigger readyToFireTrigger =
+        new Trigger(() -> launcher.isCurrent(LauncherState.PREP) && launcher.isAtGoal());
+    readyToFireTrigger.onTrue(IndexerCommands.shouldFeedCommand());
+    Trigger churnWhileFiring =
+        new Trigger(() -> launcher.isCurrent(LauncherState.PREP) && !launcher.isAtGoal());
+    churnWhileFiring.onTrue(IndexerCommands.shouldChurnCommand());
+    readyToFireTrigger
+        .negate()
+        .and(churnWhileFiring.negate())
+        .onTrue(IndexerCommands.shouldIdleCommand());
+  }
+
+  /**
+   * Returns true when it is safe to begin churning the indexer. When {@code
+   * requireFlywheelAtGoalForChurn} is {@code true} (default), churning is only permitted once the
+   * flywheel has reached its goal speed. Set the flag to {@code false} to allow churning at any
+   * time regardless of flywheel speed.
+   */
+  public static boolean isFlywheelReadyForChurn() {
+    return !requireFlywheelAtGoalForChurn
+        || (launcher != null && launcher.isFlywheelAtOrAboveGoal());
+  }
+
+  public void configureButtonBindings(Controller driver, Controller operator) {
+
+    // driver.createAButton().onTrue(shouldPrepCommand());
+    driver.createBButton().whileTrue(shouldPrepCommand()).onFalse(shouldLowCommand());
+
+    driver.createAButton().onTrue(shouldLowCommand()).onFalse(shouldLowCommand());
+
+    operator
+        .createLeftPovButton()
+        .onTrue(Commands.runOnce(() -> ShotCalculator.incrementFlywheelMultiplier(-0.01)));
+    operator
+        .createRightPovButton()
+        .onTrue(Commands.runOnce(() -> ShotCalculator.incrementFlywheelMultiplier(0.01)));
+
+    operator.createAButton().whileTrue(towerPresetStateCommand()).onFalse(shouldLowCommand());
+
+    operator
+        .createBButton()
+        .whileTrue(rightCornerPresetStateCommandr())
+        .onFalse(shouldLowCommand());
+
+    operator.createXButton().whileTrue(leftCornerPresetStateCommand()).onFalse(shouldLowCommand());
+    operator
+        .createYButton()
+        .whileTrue(turretForwardPresetStateCommand())
+        .onFalse(shouldLowCommand());
+
+    // This allowed auto-hammer time
+    // Trigger isTrenchTrigger = new Trigger(() -> launcher.isNearTrench());
+    // isTrenchTrigger.onTrue(shouldAutoHammerTimeCommand()).onFalse(shouldEscapeHammerTimeCommand());
+
+    // When intake is retracting or retracted, force turret to HAMMERTIME (hopper arm interferes)
+    Trigger intakeUpTrigger =
+        new Trigger(
+            () ->
+                intake.isCurrent(IntakeState.RETRACTING)
+                    || intake.isCurrent(IntakeState.RETRACTED));
+    intakeUpTrigger.onTrue(shouldHammerTimeCommand());
+
+    // When intake deploys away from turret, return to LOW_SPEED
+    Trigger intakeDeployingTrigger =
+        new Trigger(
+            () ->
+                (intake.isCurrent(IntakeState.DEPLOYING) || intake.isCurrent(IntakeState.INTAKING))
+                    && launcher.isCurrent(LauncherState.HAMMERTIME));
+    intakeDeployingTrigger.onTrue(shouldLowCommand());
+
+    // operator
+    //     .createUpPovButton()
+    //     .onTrue(
+    //         Commands.runOnce(() -> ShotCalculator.incrementFlywheelMultiplier(0.01))
+    //             .ignoringDisable(true));
+    // operator
+    //     .createDownPovButton()
+    //     .onTrue(
+    //         Commands.runOnce(() -> ShotCalculator.incrementFlywheelMultiplier(-0.01))
+    //             .ignoringDisable(true));
+  }
+
+  /** creates command behavior for the IDLE launcher state */
+  private static Command idleStateCommand() {
+    return Commands.parallel(
+        Commands.runOnce(
+            () -> {
+              launcher.setCurrentState(LauncherState.IDLE);
+            }),
+        launcher.stopTrackingCommand());
+  }
+  /** creates command behavior for when the launcher is at low speed */
+  private static Command lowStateCommand() {
+    return Commands.parallel(
+        Commands.runOnce(
+            () -> {
+              launcher.setCurrentState(LauncherState.LOW_SPEED);
+              LEDStrip.changeSegmentPattern(
+                  ConfigConstants.ALL_LEDS, LEDStrip.getSolidPattern(Color.kGreen));
+            }),
+        launcher.trackTargetLowCommand());
+  }
+  /** creates command behavior when the launcher is at prep state */
+  private static Command prepStateCommand() {
+    return Commands.parallel(
+        Commands.runOnce(
+            () -> {
+              launcher.setCurrentState(LauncherState.PREP);
+              LEDStrip.changeSegmentPattern(
+                  ConfigConstants.ALL_LEDS, LEDStrip.getRainbowPattern(0));
+            }),
+        launcher.trackTargetCommand());
+  }
+  /** creates command behavior for when the launcher is at preset */
+  private static Command presetStateCommand() {
+    return Commands.parallel(
+        Commands.runOnce(
+            () -> {
+              launcher.setCurrentState(LauncherState.PRESET);
+            }),
+        Commands.run(
+            () -> launcher.usePresets(presetHoodAngle, presetTurretAngle, presetFlywheelSpeed)));
+  }
+
+  public static Command shouldIdleCommand() {
+    return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.IDLE));
+  }
+
+  public static Command shouldLowCommand() {
+    return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.LOW_SPEED));
+  }
+
+  public static Command shouldPrepCommand() {
+    return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.PREP));
+  }
+
+  public static Command shouldPresetCommand() {
+    return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.PRESET));
+  }
+
+  public static Command shouldAutoHammerTimeCommand() {
+    return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.AUTO_HAMMERTIME));
+  }
+
+  public static Command shouldEscapeHammerTimeCommand() {
+    return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.ESCAPE_HAMMERTIME));
+  }
+
+  public static Command shouldToggleHammerTimeCommand() {
+    return Commands.runOnce(
+        () -> {
+          if (launcher.getCurrentState() == LauncherState.HAMMERTIME) {
+            launcher.setRequestedState(LauncherState.LOW_SPEED);
+          } else {
+            launcher.setRequestedState(LauncherState.HAMMERTIME);
+          }
+        });
+  }
+
+  public static Command shouldHammerTimeCommand() {
+    return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.HAMMERTIME));
+  }
+
+  // Order is Hood Angle, Turret Angle, Flywheel Speed
+  // Values are placeholders and need to be tuned
+  public static Command leftCornerPresetStateCommand() {
+    return shouldPresetCommand()
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  ShootingParameters params =
+                      launcher.getShootingParameters(
+                          () ->
+                              AllianceFlipUtil.apply(
+                                  new Pose2d(
+                                      new Translation2d(
+                                          Inches.of(
+                                              AprilTags.aprilTagFieldLayout
+                                                      .getTagPose(31)
+                                                      .get()
+                                                      .getX()
+                                                  + 25),
+                                          FieldConstants.FIELD_WIDTH.minus(Inches.of(17.25))),
+                                      new Rotation2d())),
+                          () -> FieldConstants.Hub.topCenterPoint.toTranslation2d());
+                  presetHoodAngle = Radians.of(params.hoodAngle());
+                  presetTurretAngle = params.turretAngle().getMeasure();
+                  presetFlywheelSpeed =
+                      RPM.of(params.flywheelSpeed() * ShotCalculator.getFlywheelMultiplier());
+                }));
+  }
+
+  public static Command rightCornerPresetStateCommandr() {
+    return shouldPresetCommand()
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  ShootingParameters params =
+                      launcher.getShootingParameters(
+                          () ->
+                              AllianceFlipUtil.apply(
+                                  new Pose2d(
+                                      new Translation2d(
+                                          Inches.of(
+                                              AprilTags.aprilTagFieldLayout
+                                                      .getTagPose(31)
+                                                      .get()
+                                                      .getX()
+                                                  + 25),
+                                          Inches.of(17.5)),
+                                      new Rotation2d())),
+                          () -> FieldConstants.Hub.topCenterPoint.toTranslation2d());
+                  presetHoodAngle = Radians.of(params.hoodAngle());
+                  presetTurretAngle = params.turretAngle().getMeasure();
+                  presetFlywheelSpeed =
+                      RPM.of(params.flywheelSpeed() * ShotCalculator.getFlywheelMultiplier());
+                }));
+  }
+
+  public static Command towerPresetStateCommand() {
+    return shouldPresetCommand()
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  ShootingParameters params =
+                      launcher.getShootingParameters(
+                          () ->
+                              AllianceFlipUtil.apply(FieldConstants.Tower.face.plus(rearToCenter)),
+                          () -> FieldConstants.Hub.topCenterPoint.toTranslation2d());
+                  presetHoodAngle = Radians.of(params.hoodAngle());
+                  presetTurretAngle = Constants.Launcher.TURRET_FORWARD;
+                  presetFlywheelSpeed =
+                      RPM.of(params.flywheelSpeed() * ShotCalculator.flywheelMultiplier);
+                }));
+  }
+
+  public static Command turretForwardPresetStateCommand() {
+    return shouldPresetCommand()
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  presetHoodAngle = Constants.Launcher.FWD_HOOD_ANGLE;
+                  presetTurretAngle = Constants.Launcher.TURRET_FORWARD;
+                  presetFlywheelSpeed = Constants.Launcher.FWD_FLYWHEEL_RPM;
+                }));
+  }
+
+  public static Command hammerTimeStateCommand() {
+    return Commands.parallel(
+        Commands.run(
+            () -> {
+              launcher.setCurrentState(LauncherState.HAMMERTIME);
+              launcher.usePresets(
+                  Constants.Launcher.LOW_HOOD_ANGLE,
+                  Degrees.of(0),
+                  Constants.Launcher.LOW_FLYWHEEL_RPM);
+            }));
+  }
+
+  public static Command autoHammerTimeStateCommand() {
+    return Commands.parallel(
+        Commands.run(
+            () -> {
+              launcher.setCurrentState(LauncherState.AUTO_HAMMERTIME);
+              launcher.usePresets(
+                  Constants.Launcher.LOW_HOOD_ANGLE,
+                  Degrees.of(0),
+                  Constants.Launcher.LOW_FLYWHEEL_RPM);
+            }));
+  }
+
+  public static Command escapeHammerTimeStateCommand() {
+    return Commands.runOnce(() -> launcher.setCurrentState(LauncherState.ESCAPE_HAMMERTIME))
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  launcher.setRequestedState(launcher.getPreTrenchState());
+                }));
+  }
+
+  public static LauncherState getCurrentState() {
+    return launcher.getCurrentState();
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/NamedCommandsReg.java b/src/main/java/frc/robot/rebuilt/commands/NamedCommandsReg.java
new file mode 100644
index 00000000..92d644d6
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/NamedCommandsReg.java
@@ -0,0 +1,34 @@
+package frc.robot.rebuilt.commands;
+
+import com.pathplanner.lib.auto.NamedCommands;
+
+public class NamedCommandsReg {
+
+  public static void createNamedCommands() {
+    // Launcer
+    NamedCommands.registerCommand("launcherPrep", LauncherCommands.shouldPrepCommand());
+    NamedCommands.registerCommand("launcherPreset", LauncherCommands.shouldPresetCommand());
+    NamedCommands.registerCommand("launcherLow", LauncherCommands.shouldLowCommand());
+    NamedCommands.registerCommand("launcherIdle", LauncherCommands.shouldIdleCommand());
+    // intake
+    NamedCommands.registerCommand("intakeIntake", IntakeCommands.shouldIntaking());
+    NamedCommands.registerCommand("intakeRetracted", IntakeCommands.shouldRetracted());
+    NamedCommands.registerCommand("intakeRetracting", IntakeCommands.shouldRetracting());
+    // climb
+    NamedCommands.registerCommand("climbDescend", ClimbCommands.shouldDescendCommand());
+    NamedCommands.registerCommand("climbElevate", ClimbCommands.shouldElevateCommand());
+    NamedCommands.registerCommand("climbEnable", ClimbCommands.shouldEnableCommand());
+    NamedCommands.registerCommand("climbStop", ClimbCommands.shouldStopCommand());
+    // indexer
+    NamedCommands.registerCommand("indexerChurn", IndexerCommands.shouldChurnCommand());
+    NamedCommands.registerCommand("indexerIdle", IndexerCommands.shouldIdleCommand());
+    NamedCommands.registerCommand("indexerFeed", IndexerCommands.shouldFeedCommand());
+    // preset
+    NamedCommands.registerCommand("iForcePreset", IndexerCommands.shouldForceCommand());
+    NamedCommands.registerCommand("hubPreset", LauncherCommands.leftCornerPresetStateCommand());
+    NamedCommands.registerCommand("towerPreset", LauncherCommands.towerPresetStateCommand());
+    NamedCommands.registerCommand(
+        "towerForwardPreset", LauncherCommands.turretForwardPresetStateCommand());
+    NamedCommands.registerCommand("WaitUntilIntaking", IntakeCommands.waitUntilIntaking());
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/ShotCalibrationCommand.java b/src/main/java/frc/robot/rebuilt/commands/ShotCalibrationCommand.java
new file mode 100644
index 00000000..fc44cc36
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/ShotCalibrationCommand.java
@@ -0,0 +1,288 @@
+package frc.robot.rebuilt.commands;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.RPM;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.FieldConstants;
+import frc.robot.rebuilt.subsystems.Launcher.Launcher;
+import frc.robot.rebuilt.subsystems.Launcher.ShotCalculator;
+import org.frc5010.common.drive.GenericDrivetrain;
+import org.frc5010.common.drive.swerve.GenericSwerveDrivetrain;
+import org.frc5010.common.utils.geometry.AllianceFlipUtil;
+
+/**
+ * A streamlined command to rapidly calibrate the shooter at various distances.
+ *
+ * <p>State Machine:
+ *
+ * <ol>
+ *   <li><b>ALIGN_AND_DRIVE:</b> Automatically drives the robot to face the hub at
+ *       `currentDistance`.
+ *   <li><b>TUNE_AND_FIRE:</b> Populates dashboard with an initial guess, allows operator to tune
+ *       RPM/Hood without bizarre scaling, and allows firing.
+ *   <li><b>NEXT_DISTANCE:</b> Acknowledges operator confirmation, logs the tuned point, and backs
+ *       up by an increment.
+ * </ol>
+ */
+public class ShotCalibrationCommand extends Command {
+
+  private enum CalibrationState {
+    ALIGN_AND_DRIVE,
+    TUNE_AND_FIRE,
+    NEXT_DISTANCE
+  }
+
+  private static final String PREFIX = "ShotCal/";
+
+  private final Launcher launcher;
+  private final GenericSwerveDrivetrain drivetrain;
+  private final ShotCalculator shotCalculator;
+
+  private CalibrationState currentState = CalibrationState.ALIGN_AND_DRIVE;
+
+  // Configuration
+  private double currentDistanceMeters;
+  private final double distanceStepMeters;
+
+  // Controllers for physical robot alignment
+  private final ProfiledPIDController xController;
+  private final ProfiledPIDController yController;
+  private final ProfiledPIDController thetaController;
+
+  // Target coordinates
+  private Translation2d hubTarget;
+  private Pose2d targetPoseForDistance;
+
+  // Tuning state block
+  private boolean initialGuessPopulated = false;
+
+  public ShotCalibrationCommand(
+      Launcher launcher,
+      GenericDrivetrain drivetrain,
+      double initialDistance,
+      double distanceStep) {
+    this.launcher = launcher;
+    this.drivetrain = (GenericSwerveDrivetrain) drivetrain;
+    this.shotCalculator = ShotCalculator.getInstance();
+
+    this.currentDistanceMeters = initialDistance;
+    this.distanceStepMeters = distanceStep;
+
+    // Initialize alignment controllers based on standard DriveToPosition constants
+    xController = new ProfiledPIDController(2.0, 0, 0, new TrapezoidProfile.Constraints(0.2, 0.5));
+    yController = new ProfiledPIDController(2.0, 0, 0, new TrapezoidProfile.Constraints(0.2, 0.5));
+    thetaController =
+        new ProfiledPIDController(3.0, 0, 0, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
+
+    xController.setTolerance(0.05); // 5cm
+    yController.setTolerance(0.05); // 5cm
+    thetaController.setTolerance(0.035); // ~2 degrees
+    thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+    addRequirements(launcher, this.drivetrain);
+  }
+
+  @Override
+  public void initialize() {
+    currentState = CalibrationState.ALIGN_AND_DRIVE;
+    initialGuessPopulated = false;
+
+    // Determine the hub position
+    hubTarget = AllianceFlipUtil.apply(FieldConstants.Hub.topCenterPoint.toTranslation2d());
+
+    // Setup dashboard fields
+    SmartDashboard.putNumber(PREFIX + "Distance Step (m)", distanceStepMeters);
+    SmartDashboard.putNumber(PREFIX + "Test Hood Angle", 35.0);
+    SmartDashboard.putNumber(PREFIX + "Test Flywheel RPM", 1800.0);
+    SmartDashboard.putNumber(PREFIX + "Flywheel Multiplier", 1.0);
+    SmartDashboard.putBoolean(PREFIX + "Force Firing", false);
+    SmartDashboard.putBoolean(PREFIX + "Confirm & Next", false);
+    SmartDashboard.putBoolean(PREFIX + "Apply Guess", false);
+
+    System.out.println("[ShotCalibration] Starting interactive calibration.");
+  }
+
+  @Override
+  public void execute() {
+    SmartDashboard.putString(PREFIX + "State", currentState.name());
+    SmartDashboard.putNumber(PREFIX + "Current Target Dist", currentDistanceMeters);
+    SmartDashboard.putNumber(PREFIX + "Actual Target Dist", getActualDistance());
+
+    switch (currentState) {
+      case ALIGN_AND_DRIVE:
+        handleAlignAndDrive();
+        break;
+
+      case TUNE_AND_FIRE:
+        handleTuneAndFire();
+        break;
+
+      case NEXT_DISTANCE:
+        handleNextDistance();
+        break;
+    }
+  }
+
+  private void handleAlignAndDrive() {
+    Pose2d currentPose = drivetrain.getPoseEstimator().getCurrentPose();
+
+    // We want to be `currentDistanceMeters` away from the hub.
+    // The easiest generic way to do this is to stand on a line drawn from the hub through our
+    // current position.
+    Translation2d hubToRobot = currentPose.getTranslation().minus(hubTarget);
+    Rotation2d angleFromHub = hubToRobot.getAngle();
+
+    // The target translation is the hub, plus a vector pointing towards the robot with length =
+    // currentDistance
+    Translation2d targetTranslation = hubTarget.plus(new Translation2d(currentDistanceMeters, 0.0));
+
+    // We want the robot to face the hub. (Angle from hub + 180 deg)
+    Rotation2d targetRotation = angleFromHub.plus(Rotation2d.fromDegrees(180));
+    targetPoseForDistance = new Pose2d(targetTranslation, targetRotation);
+
+    // Drive using controllers
+    double xSpeed = xController.calculate(currentPose.getX(), targetPoseForDistance.getX());
+    double ySpeed = yController.calculate(currentPose.getY(), targetPoseForDistance.getY());
+    double thetaSpeed =
+        thetaController.calculate(
+            currentPose.getRotation().getRadians(),
+            targetPoseForDistance.getRotation().getRadians());
+
+    ChassisSpeeds speeds =
+        ChassisSpeeds.fromFieldRelativeSpeeds(
+            xSpeed, ySpeed, thetaSpeed, currentPose.getRotation());
+    drivetrain.drive(speeds);
+
+    SmartDashboard.putNumber(PREFIX + "X Error", xController.getPositionError());
+    SmartDashboard.putNumber(PREFIX + "Y Error", yController.getPositionError());
+    SmartDashboard.putNumber(PREFIX + "Theta Error", thetaController.getPositionError());
+    SmartDashboard.putBoolean(PREFIX + "X At Goal", xController.atGoal());
+    SmartDashboard.putBoolean(PREFIX + "Y At Goal", yController.atGoal());
+    SmartDashboard.putBoolean(PREFIX + "Theta At Goal", thetaController.atGoal());
+
+    // Check transition
+    if (xController.atGoal() && yController.atGoal() && thetaController.atGoal()) {
+      drivetrain.drive(new ChassisSpeeds()); // stop
+      currentState = CalibrationState.TUNE_AND_FIRE;
+      initialGuessPopulated = false;
+    }
+  }
+
+  private void handleTuneAndFire() {
+    boolean applyGuess = SmartDashboard.getBoolean(PREFIX + "Apply Guess", false);
+
+    // 1. Give an initial guess if we just arrived or if the operator requested it
+    if (!initialGuessPopulated || applyGuess) {
+      if (applyGuess) {
+        SmartDashboard.putBoolean(PREFIX + "Apply Guess", false); // reset
+      }
+
+      double multiplier = SmartDashboard.getNumber(PREFIX + "Flywheel Multiplier", 1.0);
+      double[] guess = shotCalculator.getBallisticGuess(currentDistanceMeters);
+
+      if (false && guess != null) {
+        SmartDashboard.putNumber(PREFIX + "Test Hood Angle", guess[0]);
+        SmartDashboard.putNumber(PREFIX + "Test Flywheel RPM", guess[1] * multiplier);
+      } else {
+        // Fallback interpolation from lookup if ballistic isn't set up
+        SmartDashboard.putNumber(
+            PREFIX + "Test Hood Angle",
+            shotCalculator.getLookupHoodAngleDegrees(currentDistanceMeters));
+        SmartDashboard.putNumber(
+            PREFIX + "Test Flywheel RPM",
+            shotCalculator.getLookupFlywheelSpeed(currentDistanceMeters) * multiplier);
+      }
+      initialGuessPopulated = true;
+    }
+
+    // 2. Read explicit tuning values from operator. No funny business.
+    double hoodSetpoint = SmartDashboard.getNumber(PREFIX + "Test Hood Angle", 35.0);
+    double rpmSetpoint = SmartDashboard.getNumber(PREFIX + "Test Flywheel RPM", 1800.0);
+    boolean fireRequested = SmartDashboard.getBoolean(PREFIX + "Force Firing", false);
+    boolean nextRequested = SmartDashboard.getBoolean(PREFIX + "Confirm & Next", false);
+
+    // 3. Apply literal assignments to hardware
+    // Always zero turret for straightforward distances
+    launcher.usePresets(Degrees.of(hoodSetpoint), Degrees.of(0), RPM.of(rpmSetpoint));
+
+    // Telemetry feedback
+    SmartDashboard.putNumber(PREFIX + "Actual Hood", launcher.getHoodAngleActual().in(Degrees));
+    SmartDashboard.putNumber(PREFIX + "Actual RPM", launcher.getFlywheelSpeedActual().in(RPM));
+    SmartDashboard.putBoolean(PREFIX + "Is At Goal", launcher.isAtGoal());
+
+    // 4. Handle "Force Firing" integration -> we can't fully control the indexer natively inside a
+    // standard command without parallel racing,
+    // so we assume the operator maps a secondary button to IndexerCommands.shouldFeedCommand() to
+    // actually shoot the pre-spun ball.
+    // The "Force Firing" boolean here is mostly visual if they prefer button boards.
+
+    // 5. Transition
+    if (nextRequested) {
+      SmartDashboard.putBoolean(PREFIX + "Confirm & Next", false); // reset
+
+      // Log it!
+      System.out.println(
+          String.format(
+              "[ShotCalibration] POINT LOGGED -> DISTANCE: %.2fm | HOOD: %.1f° | RPM: %.1f",
+              getActualDistance(), hoodSetpoint, rpmSetpoint));
+
+      // Save it into live memory to test immediately
+      shotCalculator.addDataPoint(
+          getActualDistance(), hoodSetpoint, rpmSetpoint, getActualDistance() / 15.0);
+
+      currentState = CalibrationState.NEXT_DISTANCE;
+    }
+  }
+
+  private void handleNextDistance() {
+    // Increment the tracking parameter
+    double step = SmartDashboard.getNumber(PREFIX + "Distance Step (m)", distanceStepMeters);
+    currentDistanceMeters += step;
+
+    // Loop back to driving state
+    currentState = CalibrationState.ALIGN_AND_DRIVE;
+  }
+
+  private double getActualDistance() {
+    Pose2d currentPose = drivetrain.getPoseEstimator().getCurrentPose();
+    return currentPose.getTranslation().getDistance(hubTarget);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    System.out.println("[ShotCalibration] command ended. Interrupted: " + interrupted);
+    drivetrain.drive(new ChassisSpeeds());
+    launcher.stopAllMotors();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false; // Manually cancelled by operator when they're done with all distances
+  }
+
+  /**
+   * Wrap the calibration command with an indexer force-feed so the operator can fire shots during
+   * tuning.
+   */
+  public static Command createWithFeed(
+      Launcher launcher,
+      GenericDrivetrain drivetrain,
+      double initialDistance,
+      double distanceStep) {
+    return Commands.parallel(
+        new ShotCalibrationCommand(launcher, drivetrain, initialDistance, distanceStep),
+        Commands.either(
+            IndexerCommands.shouldForceCommand(),
+            IndexerCommands.shouldChurnCommand(),
+            () -> SmartDashboard.getBoolean(PREFIX + "Force Firing", false)));
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/TestCommands.java b/src/main/java/frc/robot/rebuilt/commands/TestCommands.java
new file mode 100644
index 00000000..2b9243d0
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/TestCommands.java
@@ -0,0 +1,87 @@
+package frc.robot.rebuilt.commands;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.Constants;
+// import frc.robot.rebuilt.subsystems.Climb.Climb;
+import frc.robot.rebuilt.subsystems.Indexer.Indexer;
+import frc.robot.rebuilt.subsystems.Launcher.Launcher;
+import frc.robot.rebuilt.subsystems.intake.Intake;
+import java.util.Map;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.sensors.Controller;
+
+public class TestCommands {
+
+  private Map<String, GenericSubsystem> subsystems;
+
+  Indexer indexer;
+  // Climb climb;
+  Intake intake;
+  static Launcher launcher;
+
+  public TestCommands(Map<String, GenericSubsystem> subsystems) {
+    this.subsystems = subsystems;
+    indexer = (Indexer) subsystems.get(Constants.INDEXER);
+    // climb = (Climb) subsystems.get(Constants.CLIMB);
+    intake = (Intake) subsystems.get(Constants.INTAKE);
+    launcher = (Launcher) subsystems.get(Constants.LAUNCHER);
+  }
+
+  public void configureButtonBindings(Controller controller) {
+    controller.setRightYAxis(controller.createRightYAxis().negate().deadzone(0.07));
+    controller.setLeftYAxis(controller.createLeftYAxis().negate().deadzone(0.07));
+    launcher.setDefaultCommand(launcher.getDefaultCommand());
+    intake.setDefaultCommand(
+        Commands.run(
+            () -> {
+              intake.runHopper(controller.getRightYAxis());
+              intake.runSpintake(controller.getLeftYAxis());
+            },
+            intake));
+
+    indexer.configTestControls(controller);
+    // intake.configTestController(controller);
+    // climb.configTestControls(controller);
+    controller
+        .createBButton()
+        .whileTrue(launcher.getTurretSysIdCommand().finallyDo(() -> launcher.stopAllMotors()));
+    controller
+        .createAButton()
+        .whileTrue(launcher.getFlyWheelSysIdCommand().finallyDo(() -> launcher.stopAllMotors()));
+
+    // SmartTurret tuning commands
+    controller
+        .createXButton()
+        .whileTrue(
+            launcher.getTurretQuasistaticCommand().finallyDo(() -> launcher.stopAllMotors()));
+    controller
+        .createYButton()
+        .whileTrue(launcher.getTurretKsMapCommand().finallyDo(() -> launcher.stopAllMotors()));
+    controller
+        .createLeftBumper()
+        .whileTrue(launcher.getTurretDynamicCommand().finallyDo(() -> launcher.stopAllMotors()));
+    controller
+        .createRightBumper()
+        .whileTrue(
+            launcher.getTurretTrackingTuneCommand().finallyDo(() -> launcher.stopAllMotors()));
+
+    // Seeking (MotionMagicExpo) tuning — D-pad up to hold
+    controller
+        .createUpPovButton()
+        .whileTrue(
+            launcher.getTurretSeekingTuneCommand().finallyDo(() -> launcher.stopAllMotors()));
+
+    // // Turret PID tuning — hold right bumper to enter tuning mode
+    // controller
+    //     .createRightBumper()
+    //     .whileTrue(new TurretTuningCommand(launcher).finallyDo(() -> launcher.stopAllMotors()));
+
+    // // D-pad left/right cycles turret presets while tuning
+    // controller
+    //     .createRightPovButton()
+    //     .onTrue(Commands.runOnce(() -> TurretTuningCommand.nextPreset()));
+    // controller
+    //     .createLeftPovButton()
+    //     .onTrue(Commands.runOnce(() -> TurretTuningCommand.previousPreset()));
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretDynamicCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretDynamicCommand.java
new file mode 100644
index 00000000..67344b05
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/TurretDynamicCommand.java
@@ -0,0 +1,337 @@
+package frc.robot.rebuilt.commands;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.TorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.concurrent.ConcurrentLinkedQueue;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Dynamic feedforward characterization for the turret using bidirectional current pulses.
+ *
+ * <p>Applies alternating positive and negative current steps to create many acceleration transients
+ * in both directions. The bidirectional pulsing keeps the turret oscillating near its starting
+ * position so more transients can be recorded before hitting a soft limit.
+ *
+ * <p>Two distinct quantities are measured from the same dataset:
+ *
+ * <ul>
+ *   <li><b>kS_dynamic</b>: kinetic (dynamic) friction during motion. Computed from near-steady-
+ *       state samples (|acceleration| below {@link #STEADY_STATE_ACCEL_THRESHOLD}) where {@code
+ *       kS_dyn = sgn(v) * (current - kV * velocity)}. This is often different from the static kS
+ *       measured by the quasistatic command.
+ *   <li><b>kA</b>: inertia feedforward. Computed from transient samples (|acceleration| above
+ *       {@link #MIN_ACCEL_ROT_PER_SEC_SQ}) using kS_dynamic in the residual: {@code (current -
+ *       kS_dyn * sgn(v) - kV * v) = kA * acceleration}.
+ * </ul>
+ *
+ * <p>kS_dynamic is written back to the SmartDashboard key {@code "Dynamic kS"} at the end of each
+ * run so it is automatically available for the next run's input (closing the tuning loop).
+ *
+ * <p>Data is collected at 250 Hz via a dedicated {@link Notifier}. Results are logged under the
+ * {@code "TurretDynamic/"} prefix.
+ */
+public class TurretDynamicCommand extends Command {
+
+  private final SmartTurretController controller;
+  private final TalonFX talonFX;
+  private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0);
+  private final Timer timer = new Timer();
+
+  // High-frequency status signals from SmartTurretController (250 Hz on CANivore).
+  private final StatusSignal<Angle> positionSignal;
+  private final StatusSignal<AngularVelocity> velocitySignal;
+  private final StatusSignal<AngularAcceleration> accelerationSignal;
+  private final StatusSignal<Current> torqueCurrentSignal;
+
+  // 250 Hz sampling infrastructure.
+  private final ConcurrentLinkedQueue<CharacterizationSample> sampleQueue =
+      new ConcurrentLinkedQueue<>();
+  private Notifier samplingNotifier;
+  private volatile boolean recording = false;
+
+  private final double lowerLimitRot;
+  private final double upperLimitRot;
+  private boolean positionSafetyTriggered = false;
+
+  // Values read from SmartDashboard at command start.
+  private double inputKV;
+  private double stepAmps;
+
+  /** Time to hold zero current before applying pulses. */
+  private static final double SETTLE_DELAY_SECONDS = 1.0;
+
+  /** Default step current (Amps). Configurable via SmartDashboard. */
+  private static final double DEFAULT_STEP_AMPS = 50.0;
+
+  /**
+   * Duration of each current pulse in seconds. Short enough for multiple oscillation cycles, long
+   * enough to generate a clean acceleration transient at each direction change.
+   */
+  private static final double PULSE_DURATION_SECS = 0.08;
+
+  /**
+   * Acceleration threshold below which a sample is considered steady-state (used for kS_dynamic).
+   * Units: mechanism rot/s^2.
+   */
+  private static final double STEADY_STATE_ACCEL_THRESHOLD = 0.3;
+
+  /**
+   * Minimum acceleration magnitude for transient samples used in kA regression. Units: mechanism
+   * rot/s^2.
+   */
+  private static final double MIN_ACCEL_ROT_PER_SEC_SQ = 0.5;
+
+  /** Minimum velocity magnitude to include a sample (filters near-zero noise). */
+  private static final double MIN_VELOCITY_ROT_PER_SEC = 0.05;
+
+  /** Safety margin from soft limits in mechanism rotations. */
+  private static final double POSITION_SAFETY_MARGIN_ROT = 5.0 / 360.0;
+
+  /** Sampling period for the high-frequency Notifier (4 ms = 250 Hz). */
+  private static final double SAMPLING_PERIOD_SECONDS = 1.0 / 250.0;
+
+  private static final String PREFIX = "TurretDynamic/";
+
+  public TurretDynamicCommand(SmartTurretController controller, GenericSubsystem requirement) {
+    this.controller = controller;
+    this.talonFX = controller.getTalonFX();
+
+    this.positionSignal = controller.getPositionSignal();
+    this.velocitySignal = controller.getVelocitySignal();
+    this.accelerationSignal = controller.getAccelerationSignal();
+    this.torqueCurrentSignal = controller.getTorqueCurrentSignal();
+    this.lowerLimitRot = controller.getConfig().getLowerLimitRotations();
+    this.upperLimitRot = controller.getConfig().getUpperLimitRotations();
+    addRequirements(requirement);
+  }
+
+  @Override
+  public void initialize() {
+    controller.stop();
+    timer.restart();
+    sampleQueue.clear();
+    positionSafetyTriggered = false;
+    recording = false;
+
+    // Publish defaults — kV comes from the controller config; kS is not needed as an input
+    // because this command measures kS_dynamic directly from the data.
+    SmartDashboard.putNumber(
+        "Dynamic kV", SmartDashboard.getNumber("Dynamic kV", controller.getConfig().getKV()));
+    SmartDashboard.putNumber(
+        "Dynamic Step Amps", SmartDashboard.getNumber("Dynamic Step Amps", DEFAULT_STEP_AMPS));
+
+    inputKV = SmartDashboard.getNumber("Dynamic kV", controller.getConfig().getKV());
+    stepAmps = SmartDashboard.getNumber("Dynamic Step Amps", DEFAULT_STEP_AMPS);
+
+    System.out.println("[TurretDynamic] Step=" + stepAmps + " A, kV=" + inputKV);
+    System.out.println(
+        "[TurretDynamic] Bidirectional pulsing: "
+            + PULSE_DURATION_SECS
+            + "s pulses. kS_dynamic will be computed from data.");
+
+    samplingNotifier = new Notifier(this::collectSample);
+    samplingNotifier.setName("TurretDynamicSampler");
+    samplingNotifier.startPeriodic(SAMPLING_PERIOD_SECONDS);
+  }
+
+  /** Called at 250 Hz by the Notifier. Refreshes signals and enqueues a sample. */
+  private void collectSample() {
+    if (!recording) return;
+
+    BaseStatusSignal.refreshAll(
+        positionSignal, velocitySignal, accelerationSignal, torqueCurrentSignal);
+
+    sampleQueue.add(
+        new CharacterizationSample(
+            RobotController.getFPGATime() / 1e6,
+            positionSignal.getValueAsDouble(),
+            velocitySignal.getValueAsDouble(),
+            accelerationSignal.getValueAsDouble(),
+            torqueCurrentSignal.getValueAsDouble()));
+  }
+
+  @Override
+  public void execute() {
+    double elapsed = timer.get();
+    if (elapsed < SETTLE_DELAY_SECONDS) {
+      talonFX.setControl(torqueRequest.withOutput(0));
+      recording = false;
+      Logger.recordOutput(PREFIX + "State", "Settling");
+      return;
+    }
+
+    // Safety check (uses cached signal value).
+    double actualPos = positionSignal.getValueAsDouble();
+    if (actualPos > (upperLimitRot - POSITION_SAFETY_MARGIN_ROT)
+        || actualPos < (lowerLimitRot + POSITION_SAFETY_MARGIN_ROT)) {
+      positionSafetyTriggered = true;
+      talonFX.setControl(torqueRequest.withOutput(0));
+      recording = false;
+      Logger.recordOutput(PREFIX + "State", "Position Safety");
+      return;
+    }
+
+    // Bidirectional pulsing: alternate polarity every PULSE_DURATION_SECS.
+    // Each direction change creates a fresh acceleration transient and near-zero-accel
+    // steady-state.
+    double pulseElapsed = elapsed - SETTLE_DELAY_SECONDS;
+    int pulseIndex = (int) (pulseElapsed / PULSE_DURATION_SECS);
+    double appliedAmps = (pulseIndex % 2 == 0) ? stepAmps : -stepAmps;
+
+    talonFX.setControl(torqueRequest.withOutput(appliedAmps));
+    recording = true;
+
+    Logger.recordOutput(PREFIX + "Applied Current (A)", appliedAmps);
+    Logger.recordOutput(PREFIX + "Velocity (rot-s)", velocitySignal.getValueAsDouble());
+    Logger.recordOutput(PREFIX + "Acceleration (rot-s2)", accelerationSignal.getValueAsDouble());
+    Logger.recordOutput(PREFIX + "Position (rot)", actualPos);
+    Logger.recordOutput(PREFIX + "PulseIndex", pulseIndex);
+    Logger.recordOutput(PREFIX + "SampleCount", sampleQueue.size());
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    recording = false;
+    if (samplingNotifier != null) {
+      samplingNotifier.stop();
+      samplingNotifier.close();
+      samplingNotifier = null;
+    }
+    talonFX.setControl(torqueRequest.withOutput(0));
+    controller.stop();
+
+    if (positionSafetyTriggered) {
+      Logger.recordOutput(PREFIX + "Status", "Position safety triggered — partial data");
+      System.out.println("[TurretDynamic] Ended: position safety triggered (using collected data)");
+    }
+
+    // Drain and filter samples: require minimum velocity to avoid near-zero noise.
+    List<CharacterizationSample> allSamples = new ArrayList<>();
+    CharacterizationSample s;
+    while ((s = sampleQueue.poll()) != null) {
+      if (Math.abs(s.velocityRotPerSec()) > MIN_VELOCITY_ROT_PER_SEC) {
+        allSamples.add(s);
+      }
+    }
+
+    if (allSamples.size() < 10) {
+      Logger.recordOutput(PREFIX + "Status", "Not enough samples (" + allSamples.size() + ")");
+      System.out.println("[TurretDynamic] Not enough samples: " + allSamples.size());
+      return;
+    }
+
+    // ---- Pass 1: Compute kS_dynamic from near-steady-state samples -------------------------
+    // At near-constant velocity: current ≈ kS_dyn * sgn(v) + kV * v
+    // → kS_dyn estimate per sample = sgn(v) * (current - kV * v)
+    List<Double> ksDynEstimates = new ArrayList<>();
+    for (CharacterizationSample sample : allSamples) {
+      if (Math.abs(sample.accelerationRotPerSecSq()) < STEADY_STATE_ACCEL_THRESHOLD) {
+        double ksDynEst =
+            Math.signum(sample.velocityRotPerSec())
+                * (sample.currentAmps() - inputKV * sample.velocityRotPerSec());
+        ksDynEstimates.add(ksDynEst);
+      }
+    }
+
+    double kSDynamic;
+    if (ksDynEstimates.size() >= 5) {
+      kSDynamic = ksDynEstimates.stream().mapToDouble(Double::doubleValue).average().orElse(0);
+    } else {
+      // Not enough steady-state samples — fall back to the controller config kS.
+      kSDynamic = controller.getConfig().getKS();
+      Logger.recordOutput(
+          PREFIX + "kS_dynamic Status",
+          "Insufficient steady-state samples (" + ksDynEstimates.size() + "), using config kS");
+      System.out.println(
+          "[TurretDynamic] Not enough steady-state samples for kS_dynamic ("
+              + ksDynEstimates.size()
+              + "); using config kS = "
+              + kSDynamic);
+    }
+
+    // ---- Pass 2: Compute kA from transient samples using kS_dynamic -----------------------
+    // Model: current - kS_dyn * sgn(v) - kV * v = kA * acceleration
+    // 1-parameter LS: kA = sum(residual * a) / sum(a^2)
+    double sumRA = 0, sumAA = 0;
+    int transientCount = 0;
+    for (CharacterizationSample sample : allSamples) {
+      if (Math.abs(sample.accelerationRotPerSecSq()) > MIN_ACCEL_ROT_PER_SEC_SQ) {
+        double residual =
+            sample.currentAmps()
+                - kSDynamic * Math.signum(sample.velocityRotPerSec())
+                - inputKV * sample.velocityRotPerSec();
+        double a = sample.accelerationRotPerSecSq();
+        sumRA += residual * a;
+        sumAA += a * a;
+        transientCount++;
+      }
+    }
+
+    if (transientCount < 5 || Math.abs(sumAA) < 1e-12) {
+      Logger.recordOutput(
+          PREFIX + "Status", "Not enough transient samples for kA (" + transientCount + ")");
+      System.out.println("[TurretDynamic] Not enough transient samples: " + transientCount);
+      // Still output kS_dynamic even if kA failed.
+      outputKSDynamic(kSDynamic, ksDynEstimates.size());
+      return;
+    }
+
+    double kA = sumRA / sumAA;
+
+    // ---- Output results -------------------------------------------------------------------
+    Logger.recordOutput(PREFIX + "kS_dynamic (Amps)", kSDynamic);
+    Logger.recordOutput(PREFIX + "kS_dynamic Sample Count", ksDynEstimates.size());
+    Logger.recordOutput(PREFIX + "kA (Amps per rot-s2)", kA);
+    Logger.recordOutput(PREFIX + "kA Sample Count", transientCount);
+    Logger.recordOutput(PREFIX + "Input kV (Amps per rot-s)", inputKV);
+    Logger.recordOutput(PREFIX + "Total Sample Count", allSamples.size());
+    Logger.recordOutput(PREFIX + "Status", "Complete");
+
+    System.out.println(
+        "[TurretDynamic] kS_dynamic = "
+            + kSDynamic
+            + " A  ("
+            + ksDynEstimates.size()
+            + " samples)");
+    System.out.println(
+        "[TurretDynamic] kA         = " + kA + " A/(rot/s^2)  (" + transientCount + " samples)");
+    System.out.println(
+        "[TurretDynamic] Used kV=" + inputKV + ", total samples=" + allSamples.size());
+
+    outputKSDynamic(kSDynamic, ksDynEstimates.size());
+  }
+
+  /**
+   * Publishes kS_dynamic to the SmartDashboard key {@code "Dynamic kS"} so it is available as the
+   * starting value for the next characterization run and for use in other commands.
+   */
+  private void outputKSDynamic(double kSDynamic, int sampleCount) {
+    SmartDashboard.putNumber("Dynamic kS", kSDynamic);
+    System.out.println(
+        "[TurretDynamic] kS_dynamic="
+            + kSDynamic
+            + " A written to SmartDashboard 'Dynamic kS' ("
+            + sampleCount
+            + " steady-state samples).");
+  }
+
+  @Override
+  public boolean isFinished() {
+    return positionSafetyTriggered;
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretKsMapCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretKsMapCommand.java
new file mode 100644
index 00000000..309b5f9b
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/TurretKsMapCommand.java
@@ -0,0 +1,219 @@
+package frc.robot.rebuilt.commands;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.TorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Generates a position-dependent kS map for the turret by probing at multiple positions.
+ *
+ * <p>At each test position, ramps TorqueCurrentFOC in both positive and negative directions until
+ * movement is detected, recording the minimum current needed. The result is two {@link
+ * InterpolatingDoubleTreeMap}s (positive and negative direction) that can be injected into the
+ * SmartTurretController for dynamic kS compensation.
+ */
+public class TurretKsMapCommand extends Command {
+
+  private enum State {
+    MOVE_TO_POSITION,
+    SETTLE,
+    PROBE_POSITIVE,
+    PROBE_NEGATIVE,
+    RECORD_AND_ADVANCE,
+    DONE
+  }
+
+  private final SmartTurretController controller;
+  private final TalonFX talonFX;
+  private final StatusSignal<Angle> positionSignal;
+  private final StatusSignal<AngularVelocity> velocitySignal;
+  private final MotionMagicTorqueCurrentFOC moveRequest =
+      new MotionMagicTorqueCurrentFOC(0).withSlot(0);
+  private final TorqueCurrentFOC probeRequest = new TorqueCurrentFOC(0);
+  private final Timer settleTimer = new Timer();
+
+  private final double[] testPositions;
+  private int currentIndex = 0;
+  private State currentState = State.MOVE_TO_POSITION;
+
+  private double probeCurrent = 0.0;
+  private double ksPositive = 0.0;
+  private double ksNegative = 0.0;
+
+  private static final double PROBE_RAMP_RATE_AMPS_PER_CYCLE = 0.05; // Amps per 20ms cycle
+  private static final double MOVEMENT_THRESHOLD_ROT_PER_SEC = 0.005;
+  private static final double POSITION_TOLERANCE_ROT = 0.01;
+  private static final double SETTLE_TIME_SECONDS = 0.5;
+  private static final double MAX_PROBE_CURRENT_AMPS = 30.0;
+  private static final int NUM_TEST_POSITIONS = 10;
+  private static final String PREFIX = "TurretKsMap/";
+
+  private final InterpolatingDoubleTreeMap resultMapPositive = new InterpolatingDoubleTreeMap();
+  private final InterpolatingDoubleTreeMap resultMapNegative = new InterpolatingDoubleTreeMap();
+
+  public TurretKsMapCommand(
+      SmartTurretController controller,
+      Angle lowerLimit,
+      Angle upperLimit,
+      GenericSubsystem requirement) {
+    this.controller = controller;
+    this.talonFX = controller.getTalonFX();
+    this.positionSignal = controller.getPositionSignal();
+    this.velocitySignal = controller.getVelocitySignal();
+    addRequirements(requirement);
+
+    // Generate evenly-spaced test positions across the turret range.
+    double lowerRot = lowerLimit.in(edu.wpi.first.units.Units.Rotations);
+    double upperRot = upperLimit.in(edu.wpi.first.units.Units.Rotations);
+    testPositions = new double[NUM_TEST_POSITIONS];
+    for (int i = 0; i < NUM_TEST_POSITIONS; i++) {
+      testPositions[i] = lowerRot + (upperRot - lowerRot) * i / (NUM_TEST_POSITIONS - 1);
+    }
+  }
+
+  @Override
+  public void initialize() {
+    // Disable the SmartTurretController so the 200Hz Notifier stops sending competing commands.
+    controller.stop();
+    currentIndex = 0;
+    currentState = State.MOVE_TO_POSITION;
+    resultMapPositive.clear();
+    resultMapNegative.clear();
+    Logger.recordOutput(PREFIX + "Status", "Running");
+  }
+
+  @Override
+  public void execute() {
+    // Refresh signals for latest 250 Hz data.
+    BaseStatusSignal.refreshAll(positionSignal, velocitySignal);
+    double actualPos = positionSignal.getValueAsDouble();
+    double actualVel = velocitySignal.getValueAsDouble();
+
+    Logger.recordOutput(PREFIX + "State", currentState.name());
+    Logger.recordOutput(PREFIX + "Position Index", currentIndex);
+    Logger.recordOutput(PREFIX + "Probe Current", probeCurrent);
+
+    switch (currentState) {
+      case MOVE_TO_POSITION:
+        talonFX.setControl(moveRequest.withPosition(testPositions[currentIndex]));
+        if (Math.abs(actualPos - testPositions[currentIndex]) < POSITION_TOLERANCE_ROT
+            && Math.abs(actualVel) < MOVEMENT_THRESHOLD_ROT_PER_SEC) {
+          settleTimer.restart();
+          currentState = State.SETTLE;
+        }
+        break;
+
+      case SETTLE:
+        talonFX.setControl(probeRequest.withOutput(0));
+        if (settleTimer.hasElapsed(SETTLE_TIME_SECONDS)) {
+          probeCurrent = 0;
+          currentState = State.PROBE_POSITIVE;
+        }
+        break;
+
+      case PROBE_POSITIVE:
+        probeCurrent += PROBE_RAMP_RATE_AMPS_PER_CYCLE;
+        talonFX.setControl(probeRequest.withOutput(probeCurrent));
+        if (Math.abs(velocitySignal.getValueAsDouble()) > MOVEMENT_THRESHOLD_ROT_PER_SEC) {
+          ksPositive = probeCurrent;
+          probeCurrent = 0;
+          settleTimer.restart();
+          currentState = State.PROBE_NEGATIVE;
+          // Return to position first.
+          talonFX.setControl(probeRequest.withOutput(0));
+        } else if (probeCurrent > MAX_PROBE_CURRENT_AMPS) {
+          ksPositive = MAX_PROBE_CURRENT_AMPS;
+          probeCurrent = 0;
+          settleTimer.restart();
+          currentState = State.PROBE_NEGATIVE;
+          talonFX.setControl(probeRequest.withOutput(0));
+        }
+        break;
+
+      case PROBE_NEGATIVE:
+        if (!settleTimer.hasElapsed(SETTLE_TIME_SECONDS)) {
+          talonFX.setControl(probeRequest.withOutput(0));
+          return;
+        }
+        probeCurrent -= PROBE_RAMP_RATE_AMPS_PER_CYCLE;
+        talonFX.setControl(probeRequest.withOutput(probeCurrent));
+        if (Math.abs(velocitySignal.getValueAsDouble()) > MOVEMENT_THRESHOLD_ROT_PER_SEC) {
+          ksNegative = Math.abs(probeCurrent);
+          probeCurrent = 0;
+          currentState = State.RECORD_AND_ADVANCE;
+        } else if (Math.abs(probeCurrent) > MAX_PROBE_CURRENT_AMPS) {
+          ksNegative = MAX_PROBE_CURRENT_AMPS;
+          probeCurrent = 0;
+          currentState = State.RECORD_AND_ADVANCE;
+        }
+        break;
+
+      case RECORD_AND_ADVANCE:
+        talonFX.setControl(probeRequest.withOutput(0));
+        resultMapPositive.put(testPositions[currentIndex], ksPositive);
+        resultMapNegative.put(testPositions[currentIndex], ksNegative);
+        System.out.printf(
+            "[TurretKsMap] Position %.4f rot: kS+ = %.3f A, kS- = %.3f A%n",
+            testPositions[currentIndex], ksPositive, ksNegative);
+        Logger.recordOutput(
+            PREFIX + "kS+ @ " + String.format("%.1f", testPositions[currentIndex] * 360) + "deg",
+            ksPositive);
+        Logger.recordOutput(
+            PREFIX + "kS- @ " + String.format("%.1f", testPositions[currentIndex] * 360) + "deg",
+            ksNegative);
+
+        currentIndex++;
+        if (currentIndex < testPositions.length) {
+          currentState = State.MOVE_TO_POSITION;
+        } else {
+          currentState = State.DONE;
+        }
+        break;
+
+      case DONE:
+        break;
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    talonFX.setControl(probeRequest.withOutput(0));
+    // Re-disable the controller so the turret doesn't chase a stale target.
+    controller.stop();
+    if (currentState == State.DONE) {
+      Logger.recordOutput(PREFIX + "Status", "Complete");
+      System.out.println("[TurretKsMap] Mapping complete. Results:");
+      for (int i = 0; i < testPositions.length; i++) {
+        double pos = testPositions[i];
+        System.out.printf(
+            "  %.1f deg: kS+ = %.3f A, kS- = %.3f A%n",
+            pos * 360, resultMapPositive.get(pos), resultMapNegative.get(pos));
+      }
+    } else {
+      Logger.recordOutput(PREFIX + "Status", "Interrupted at index " + currentIndex);
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return currentState == State.DONE;
+  }
+
+  public InterpolatingDoubleTreeMap getResultMapPositive() {
+    return resultMapPositive;
+  }
+
+  public InterpolatingDoubleTreeMap getResultMapNegative() {
+    return resultMapNegative;
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretQuasistaticCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretQuasistaticCommand.java
new file mode 100644
index 00000000..952c7782
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/TurretQuasistaticCommand.java
@@ -0,0 +1,219 @@
+package frc.robot.rebuilt.commands;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.TorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.concurrent.ConcurrentLinkedQueue;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Quasistatic feedforward characterization for the turret using {@link TorqueCurrentFOC}.
+ *
+ * <p>Ramps torque current very slowly so the turret reaches near-steady-state velocity at each
+ * sample. Fits a 2-parameter model: {@code current = kS + kV * velocity} via least-squares
+ * regression. Samples are only recorded once velocity exceeds a threshold (turret is moving).
+ *
+ * <p>Data is collected at ~250 Hz via a dedicated {@link Notifier}, not at the 50 Hz robot loop.
+ * This provides ~5x more samples for significantly smoother regression data.
+ *
+ * <p>Results are logged under the "TurretQuasistatic/" prefix.
+ */
+public class TurretQuasistaticCommand extends Command {
+
+  private final SmartTurretController controller;
+  private final TalonFX talonFX;
+  private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0);
+  private final Timer timer = new Timer();
+
+  // High-frequency status signals from SmartTurretController (250 Hz on CANivore).
+  private final StatusSignal<Angle> positionSignal;
+  private final StatusSignal<AngularVelocity> velocitySignal;
+  private final StatusSignal<AngularAcceleration> accelerationSignal;
+  private final StatusSignal<Current> torqueCurrentSignal;
+
+  // 250 Hz sampling infrastructure.
+  private final ConcurrentLinkedQueue<CharacterizationSample> sampleQueue =
+      new ConcurrentLinkedQueue<>();
+  private Notifier samplingNotifier;
+  private volatile boolean recording = false;
+
+  private final double lowerLimitRot;
+  private final double upperLimitRot;
+  private boolean positionSafetyTriggered = false;
+
+  /** Slow ramp rate so velocity has time to reach steady state at each current level. */
+  private static final double RAMP_RATE_AMPS_PER_SEC = 0.5;
+
+  /** Time to wait before ramping, letting the system settle. */
+  private static final double SETTLE_DELAY_SECONDS = 1.0;
+
+  /** Minimum velocity before we include a sample in the regression (turret must be moving). */
+  private static final double MIN_VELOCITY_ROT_PER_SEC = 0.01;
+
+  /** Safety margin from soft limits in mechanism rotations. */
+  private static final double POSITION_SAFETY_MARGIN_ROT = 5.0 / 360.0;
+
+  /** Sampling period for the high-frequency Notifier (4 ms = 250 Hz). */
+  private static final double SAMPLING_PERIOD_SECONDS = 1.0 / 250.0;
+
+  private static final String PREFIX = "TurretQuasistatic/";
+
+  public TurretQuasistaticCommand(SmartTurretController controller, GenericSubsystem requirement) {
+    this.controller = controller;
+    this.talonFX = controller.getTalonFX();
+    this.positionSignal = controller.getPositionSignal();
+    this.velocitySignal = controller.getVelocitySignal();
+    this.accelerationSignal = controller.getAccelerationSignal();
+    this.torqueCurrentSignal = controller.getTorqueCurrentSignal();
+    this.lowerLimitRot = controller.getConfig().getLowerLimitRotations();
+    this.upperLimitRot = controller.getConfig().getUpperLimitRotations();
+    addRequirements(requirement);
+  }
+
+  @Override
+  public void initialize() {
+    controller.stop();
+    timer.restart();
+    sampleQueue.clear();
+    positionSafetyTriggered = false;
+    recording = false;
+
+    // Start the 250 Hz sampling Notifier.
+    samplingNotifier = new Notifier(this::collectSample);
+    samplingNotifier.setName("TurretQuasistaticSampler");
+    samplingNotifier.startPeriodic(SAMPLING_PERIOD_SECONDS);
+  }
+
+  /** Called at 250 Hz by the Notifier. Refreshes signals and enqueues a sample. */
+  private void collectSample() {
+    if (!recording) return;
+
+    BaseStatusSignal.refreshAll(
+        positionSignal, velocitySignal, accelerationSignal, torqueCurrentSignal);
+
+    sampleQueue.add(
+        new CharacterizationSample(
+            RobotController.getFPGATime() / 1e6,
+            positionSignal.getValueAsDouble(),
+            velocitySignal.getValueAsDouble(),
+            accelerationSignal.getValueAsDouble(),
+            torqueCurrentSignal.getValueAsDouble()));
+  }
+
+  @Override
+  public void execute() {
+    double elapsed = timer.get();
+    if (elapsed < SETTLE_DELAY_SECONDS) {
+      talonFX.setControl(torqueRequest.withOutput(0));
+      recording = false;
+      Logger.recordOutput(PREFIX + "State", "Settling");
+      return;
+    }
+
+    // Safety check (uses cached signal value — no refreshAll needed at 50 Hz).
+    double actualPos = positionSignal.getValueAsDouble();
+    if (actualPos > (upperLimitRot - POSITION_SAFETY_MARGIN_ROT)
+        || actualPos < (lowerLimitRot + POSITION_SAFETY_MARGIN_ROT)) {
+      positionSafetyTriggered = true;
+      talonFX.setControl(torqueRequest.withOutput(0));
+      recording = false;
+      Logger.recordOutput(PREFIX + "State", "Position Safety");
+      return;
+    }
+
+    double rampTime = elapsed - SETTLE_DELAY_SECONDS;
+    double current = rampTime * RAMP_RATE_AMPS_PER_SEC;
+    talonFX.setControl(torqueRequest.withOutput(current));
+    recording = true;
+
+    // 50 Hz dashboard telemetry for live viewing.
+    Logger.recordOutput(PREFIX + "Current (A)", current);
+    Logger.recordOutput(PREFIX + "Velocity (rot-s)", velocitySignal.getValueAsDouble());
+    Logger.recordOutput(PREFIX + "Position (rot)", actualPos);
+    Logger.recordOutput(PREFIX + "SampleCount", sampleQueue.size());
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    // Stop sampling first, then stop the motor.
+    recording = false;
+    if (samplingNotifier != null) {
+      samplingNotifier.stop();
+      samplingNotifier.close();
+      samplingNotifier = null;
+    }
+    talonFX.setControl(torqueRequest.withOutput(0));
+    controller.stop();
+
+    if (positionSafetyTriggered) {
+      Logger.recordOutput(PREFIX + "Status", "Position safety triggered");
+      System.out.println("[TurretQuasistatic] Ended: position safety triggered");
+    }
+
+    // Drain the queue, filtering for samples where the turret is actually moving.
+    List<CharacterizationSample> samples = new ArrayList<>();
+    CharacterizationSample s;
+    while ((s = sampleQueue.poll()) != null) {
+      if (Math.abs(s.velocityRotPerSec()) > MIN_VELOCITY_ROT_PER_SEC) {
+        samples.add(s);
+      }
+    }
+
+    if (samples.size() < 10) {
+      Logger.recordOutput(PREFIX + "Status", "Not enough samples (" + samples.size() + ")");
+      System.out.println("[TurretQuasistatic] Not enough samples: " + samples.size());
+      return;
+    }
+
+    // 2-parameter least-squares fit: current = kS + kV * velocity
+    // Normal equations:
+    // [n,     sum_v ] [kS]   [sum_i ]
+    // [sum_v, sum_vv] [kV] = [sum_iv]
+    double n = samples.size();
+    double sumV = 0, sumVV = 0, sumI = 0, sumIV = 0;
+
+    for (CharacterizationSample sample : samples) {
+      double v = sample.velocityRotPerSec();
+      double c = sample.currentAmps();
+      sumV += v;
+      sumVV += v * v;
+      sumI += c;
+      sumIV += c * v;
+    }
+
+    double det = n * sumVV - sumV * sumV;
+    if (Math.abs(det) < 1e-12) {
+      Logger.recordOutput(PREFIX + "Status", "Singular matrix");
+      System.out.println("[TurretQuasistatic] Singular matrix — no solution");
+      return;
+    }
+
+    double kS = (sumI * sumVV - sumV * sumIV) / det;
+    double kV = (n * sumIV - sumV * sumI) / det;
+
+    Logger.recordOutput(PREFIX + "kS (Amps)", kS);
+    Logger.recordOutput(PREFIX + "kV (Amps per rot-s)", kV);
+    Logger.recordOutput(PREFIX + "Status", "Complete (" + samples.size() + " samples)");
+
+    System.out.println("[TurretQuasistatic] kS = " + kS + " A, kV = " + kV + " A/(rot/s)");
+    System.out.println("[TurretQuasistatic] Samples: " + samples.size());
+  }
+
+  @Override
+  public boolean isFinished() {
+    return positionSafetyTriggered;
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretSeekingTuneCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretSeekingTuneCommand.java
new file mode 100644
index 00000000..ee3e7049
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/TurretSeekingTuneCommand.java
@@ -0,0 +1,175 @@
+package frc.robot.rebuilt.commands;
+
+import static edu.wpi.first.units.Units.Rotations;
+
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Tunes the SEEKING (MotionMagicExpo) mode by alternating between two target positions.
+ *
+ * <p>All Slot0 PID/FF gains and the MotionMagicExpo plant model (kV/kA in Volts) are adjustable via
+ * SmartDashboard in real-time. The target positions and dwell time at each target are also tunable.
+ *
+ * <p>This lets the operator observe overshoot, settling time, and profile shape by simply watching
+ * the turret move back and forth while tweaking gains on the dashboard.
+ */
+public class TurretSeekingTuneCommand extends Command {
+
+  private final SmartTurretController controller;
+  private final TalonFX talonFX;
+  private final Timer timer = new Timer();
+  private final Timer settleTimer = new Timer();
+
+  private boolean atTargetA = true;
+
+  // Cached Slot0 gains for change detection.
+  private double lastKP = -1, lastKI = -1, lastKD = -1;
+  private double lastKS = -1, lastKV = -1, lastKA = -1;
+  // Cached expo gains for change detection.
+  private double lastExpoKV = -1, lastExpoKA = -1;
+
+  private static final String PREFIX = "TurretSeekTune/";
+
+  public TurretSeekingTuneCommand(SmartTurretController controller, GenericSubsystem requirement) {
+    this.controller = controller;
+    this.talonFX = controller.getTalonFX();
+    addRequirements(requirement);
+  }
+
+  @Override
+  public void initialize() {
+    timer.restart();
+    settleTimer.restart();
+    atTargetA = true;
+
+    var config = controller.getConfig();
+    double lowerDeg = config.getLowerLimitRotations() * 360.0;
+    double upperDeg = config.getUpperLimitRotations() * 360.0;
+
+    // Target positions (degrees) — default to ±50% of range from center.
+    SmartDashboard.putNumber(
+        PREFIX + "Target A (deg)",
+        SmartDashboard.getNumber(PREFIX + "Target A (deg)", lowerDeg * 0.5));
+    SmartDashboard.putNumber(
+        PREFIX + "Target B (deg)",
+        SmartDashboard.getNumber(PREFIX + "Target B (deg)", upperDeg * 0.5));
+    SmartDashboard.putNumber(
+        PREFIX + "Dwell Time (s)", SmartDashboard.getNumber(PREFIX + "Dwell Time (s)", 2.0));
+
+    // Slot0 PID gains.
+    SmartDashboard.putNumber(PREFIX + "Seeking kP", config.getSeekingKP());
+    SmartDashboard.putNumber(PREFIX + "Seeking kI", config.getSeekingKI());
+    SmartDashboard.putNumber(PREFIX + "Seeking kD", config.getSeekingKD());
+    lastKP = config.getSeekingKP();
+    lastKI = config.getSeekingKI();
+    lastKD = config.getSeekingKD();
+
+    // Slot0 feedforward gains (Amps — TorqueCurrentFOC).
+    SmartDashboard.putNumber(PREFIX + "Seeking kS (A)", config.getKS());
+    SmartDashboard.putNumber(PREFIX + "Seeking kV (A/rps)", config.getKV());
+    SmartDashboard.putNumber(PREFIX + "Seeking kA (A/rps2)", config.getKA());
+    lastKS = config.getKS();
+    lastKV = config.getKV();
+    lastKA = config.getKA();
+
+    // Expo plant model (Volts — always V/rps and V/rps^2).
+    SmartDashboard.putNumber(PREFIX + "Expo kV (V/rps)", config.getExpoKV());
+    SmartDashboard.putNumber(PREFIX + "Expo kA (V/rps2)", config.getExpoKA());
+    lastExpoKV = config.getExpoKV();
+    lastExpoKA = config.getExpoKA();
+  }
+
+  @Override
+  public void execute() {
+    // Read target positions and dwell time.
+    double targetADeg = SmartDashboard.getNumber(PREFIX + "Target A (deg)", -60);
+    double targetBDeg = SmartDashboard.getNumber(PREFIX + "Target B (deg)", 60);
+    double dwellTimeSec = SmartDashboard.getNumber(PREFIX + "Dwell Time (s)", 2.0);
+
+    // Switch targets after dwell time elapses.
+    if (settleTimer.hasElapsed(dwellTimeSec)) {
+      atTargetA = !atTargetA;
+      settleTimer.restart();
+    }
+
+    double targetDeg = atTargetA ? targetADeg : targetBDeg;
+    double targetRot = targetDeg / 360.0;
+
+    // Command the turret (zero velocity/accel FF — SEEKING mode handles the profile).
+    controller.setTarget(Rotations.of(targetRot), 0, 0);
+
+    // --- Live gain updates from SmartDashboard ---
+    double newKP = SmartDashboard.getNumber(PREFIX + "Seeking kP", lastKP);
+    double newKI = SmartDashboard.getNumber(PREFIX + "Seeking kI", lastKI);
+    double newKD = SmartDashboard.getNumber(PREFIX + "Seeking kD", lastKD);
+    double newKS = SmartDashboard.getNumber(PREFIX + "Seeking kS (A)", lastKS);
+    double newKV = SmartDashboard.getNumber(PREFIX + "Seeking kV (A/rps)", lastKV);
+    double newKA = SmartDashboard.getNumber(PREFIX + "Seeking kA (A/rps2)", lastKA);
+
+    if (newKP != lastKP
+        || newKI != lastKI
+        || newKD != lastKD
+        || newKS != lastKS
+        || newKV != lastKV
+        || newKA != lastKA) {
+      Slot0Configs slot0 = new Slot0Configs();
+      slot0.kP = newKP;
+      slot0.kI = newKI;
+      slot0.kD = newKD;
+      slot0.kS = newKS;
+      slot0.kV = newKV;
+      slot0.kA = newKA;
+      talonFX.getConfigurator().apply(slot0);
+      lastKP = newKP;
+      lastKI = newKI;
+      lastKD = newKD;
+      lastKS = newKS;
+      lastKV = newKV;
+      lastKA = newKA;
+    }
+
+    // Expo plant model updates (Volts).
+    double newExpoKV = SmartDashboard.getNumber(PREFIX + "Expo kV (V/rps)", lastExpoKV);
+    double newExpoKA = SmartDashboard.getNumber(PREFIX + "Expo kA (V/rps2)", lastExpoKA);
+
+    if (newExpoKV != lastExpoKV || newExpoKA != lastExpoKA) {
+      MotionMagicConfigs mmConfig = new MotionMagicConfigs();
+      mmConfig.MotionMagicExpo_kV = newExpoKV;
+      mmConfig.MotionMagicExpo_kA = newExpoKA;
+      mmConfig.MotionMagicCruiseVelocity = controller.getConfig().getMaxVelocityMechRotPerSec();
+      talonFX.getConfigurator().apply(mmConfig);
+      lastExpoKV = newExpoKV;
+      lastExpoKA = newExpoKA;
+    }
+
+    // Logging.
+    double actualPos = controller.getActualPositionMechRot();
+    double positionError = targetRot - actualPos;
+
+    Logger.recordOutput(PREFIX + "TargetPositionDeg", targetDeg);
+    Logger.recordOutput(PREFIX + "ActualPositionDeg", actualPos * 360.0);
+    Logger.recordOutput(PREFIX + "PositionErrorDeg", positionError * 360.0);
+    Logger.recordOutput(
+        PREFIX + "ActualVelocityRPS", controller.getActualVelocityRadPerSec() / (2.0 * Math.PI));
+    Logger.recordOutput(PREFIX + "AtTargetA", atTargetA);
+    Logger.recordOutput(PREFIX + "State", controller.getCurrentTurretState().name());
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    controller.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false; // Run until interrupted.
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretTrackingTuneCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretTrackingTuneCommand.java
new file mode 100644
index 00000000..492950a3
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/commands/TurretTrackingTuneCommand.java
@@ -0,0 +1,158 @@
+package frc.robot.rebuilt.commands;
+
+import static edu.wpi.first.units.Units.Rotations;
+
+import com.ctre.phoenix6.configs.Slot1Configs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Generates a sinusoidal reference signal for the turret and measures tracking error.
+ *
+ * <p>Amplitude, frequency, PID gains (Slot1: tracking), and feedforward gains (kV, kA) are
+ * adjustable via SmartDashboard in real-time. The amplitude is automatically clamped so the
+ * sinusoidal stays within safe turret limits centered at 0. The analytically-derived velocity and
+ * acceleration of the sinusoid are passed as feedforward to the SmartTurretController.
+ *
+ * <p>Slot1 kS is always 0 in the firmware — the position-dependent kS is injected externally by the
+ * SmartTurretController. kV and kA run at full firmware frequency (1 kHz) for best performance.
+ */
+public class TurretTrackingTuneCommand extends Command {
+
+  private final SmartTurretController controller;
+  private final TalonFX talonFX;
+  private final Timer timer = new Timer();
+
+  private final double maxSafeAmplitudeRot;
+
+  private double amplitudeRotations;
+  private double frequencyHz;
+
+  // Cached gain values for change detection.
+  private double lastKP = -1, lastKI = -1, lastKD = -1;
+  private double lastKV = -1, lastKA = -1;
+
+  private static final double SAFETY_MARGIN_ROT = 10.0 / 360.0; // 10 degrees
+  private static final String PREFIX = "TurretTrackTune/";
+
+  public TurretTrackingTuneCommand(SmartTurretController controller, GenericSubsystem requirement) {
+    this.controller = controller;
+    this.talonFX = controller.getTalonFX();
+    addRequirements(requirement);
+
+    // Compute max safe amplitude: min(|lower|, |upper|) - safety margin, centered at 0.
+    double lowerLimit = Math.abs(controller.getConfig().getLowerLimitRotations());
+    double upperLimit = Math.abs(controller.getConfig().getUpperLimitRotations());
+    maxSafeAmplitudeRot = Math.max(0, Math.min(lowerLimit, upperLimit) - SAFETY_MARGIN_ROT);
+  }
+
+  @Override
+  public void initialize() {
+    timer.restart();
+
+    // Initialize SmartDashboard defaults.
+    amplitudeRotations = SmartDashboard.getNumber(PREFIX + "Amplitude (rot)", 0.1);
+    frequencyHz = SmartDashboard.getNumber(PREFIX + "Frequency (Hz)", 0.5);
+    SmartDashboard.putNumber(PREFIX + "Amplitude (rot)", amplitudeRotations);
+    SmartDashboard.putNumber(PREFIX + "Frequency (Hz)", frequencyHz);
+
+    double currentKP = controller.getConfig().getTrackingKP();
+    double currentKI = controller.getConfig().getTrackingKI();
+    double currentKD = controller.getConfig().getTrackingKD();
+    SmartDashboard.putNumber(PREFIX + "Tracking kP", currentKP);
+    SmartDashboard.putNumber(PREFIX + "Tracking kI", currentKI);
+    SmartDashboard.putNumber(PREFIX + "Tracking kD", currentKD);
+    lastKP = currentKP;
+    lastKI = currentKI;
+    lastKD = currentKD;
+
+    // Publish tunable feedforward gains (Amps). kV and kA run in the firmware slot.
+    double currentKV = controller.getConfig().getKV();
+    double currentKA = controller.getConfig().getKA();
+    SmartDashboard.putNumber(PREFIX + "Tracking kV (A/rps)", currentKV);
+    SmartDashboard.putNumber(PREFIX + "Tracking kA (A/rps2)", currentKA);
+    lastKV = currentKV;
+    lastKA = currentKA;
+
+    Logger.recordOutput(PREFIX + "MaxSafeAmplitudeRot", maxSafeAmplitudeRot);
+  }
+
+  @Override
+  public void execute() {
+    // Read tunable parameters.
+    amplitudeRotations = SmartDashboard.getNumber(PREFIX + "Amplitude (rot)", 0.1);
+    frequencyHz = SmartDashboard.getNumber(PREFIX + "Frequency (Hz)", 0.5);
+
+    // Clamp amplitude to safe range.
+    amplitudeRotations = MathUtil.clamp(amplitudeRotations, 0, maxSafeAmplitudeRot);
+
+    double t = timer.get();
+    double omega = 2.0 * Math.PI * frequencyHz;
+
+    // Sinusoidal position reference (mechanism rotations).
+    double positionRot = amplitudeRotations * Math.sin(omega * t);
+    // Analytical velocity (rad/s): d/dt [A * sin(wt)] * 2pi = A * w * cos(wt) * 2pi
+    double velocityRadPerSec = amplitudeRotations * omega * Math.cos(omega * t) * 2.0 * Math.PI;
+    // Analytical acceleration (rad/s^2): d/dt [velocity] = -A * w^2 * sin(wt) * 2pi
+    double accelRadPerSecSq =
+        -amplitudeRotations * omega * omega * Math.sin(omega * t) * 2.0 * Math.PI;
+
+    controller.setTarget(Rotations.of(positionRot), velocityRadPerSec, accelRadPerSecSq);
+
+    // Check if any gains changed on dashboard and apply Slot1 update.
+    double newKP = SmartDashboard.getNumber(PREFIX + "Tracking kP", lastKP);
+    double newKI = SmartDashboard.getNumber(PREFIX + "Tracking kI", lastKI);
+    double newKD = SmartDashboard.getNumber(PREFIX + "Tracking kD", lastKD);
+    double newKV = SmartDashboard.getNumber(PREFIX + "Tracking kV (A/rps)", lastKV);
+    double newKA = SmartDashboard.getNumber(PREFIX + "Tracking kA (A/rps2)", lastKA);
+
+    if (newKP != lastKP
+        || newKI != lastKI
+        || newKD != lastKD
+        || newKV != lastKV
+        || newKA != lastKA) {
+      Slot1Configs slot1 = new Slot1Configs();
+      slot1.kP = newKP;
+      slot1.kI = newKI;
+      slot1.kD = newKD;
+      // kS = 0: position-dependent kS is injected externally by SmartTurretController.
+      slot1.kS = 0;
+      slot1.kV = newKV;
+      slot1.kA = newKA;
+      talonFX.getConfigurator().apply(slot1);
+      lastKP = newKP;
+      lastKI = newKI;
+      lastKD = newKD;
+      lastKV = newKV;
+      lastKA = newKA;
+    }
+
+    // Log tracking data via AdvantageKit.
+    double actualPos = controller.getActualPositionMechRot();
+    double trackingError = positionRot - actualPos;
+
+    Logger.recordOutput(PREFIX + "ReferencePositionRot", positionRot);
+    Logger.recordOutput(PREFIX + "ActualPositionRot", actualPos);
+    Logger.recordOutput(PREFIX + "TrackingErrorRot", trackingError);
+    Logger.recordOutput(PREFIX + "TrackingErrorDeg", trackingError * 360.0);
+    Logger.recordOutput(PREFIX + "VelocityFFRadPerSec", velocityRadPerSec);
+    Logger.recordOutput(PREFIX + "AccelFFRadPerSecSq", accelRadPerSecSq);
+    Logger.recordOutput(PREFIX + "ClampedAmplitudeRot", amplitudeRotations);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    controller.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false; // Run until interrupted.
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Climb/Climb.java b/src/main/java/frc/robot/rebuilt/subsystems/Climb/Climb.java
new file mode 100644
index 00000000..c02a3c27
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Climb/Climb.java
@@ -0,0 +1,90 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.Climb;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.commands.ClimbCommands.ClimbState;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.sensors.Controller;
+import org.littletonrobotics.junction.Logger;
+
+/** The class climb controlls the climb */
+public class Climb extends GenericSubsystem {
+  private final ClimbIO io;
+  private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged();
+  /** Creates the climb subsystem and chooses the IO */
+  public Climb() {
+    super("climb.json");
+    if (RobotBase.isSimulation()) {
+      io = new ClimbIOSim(devices);
+    } else {
+      io = new ClimbIOReal(devices);
+    }
+  }
+  /** Sets a command that holds the climb at a given height */
+  public Command climberCommand(Distance height) {
+    return Commands.run(
+            () -> {
+              setClimbHeight(height);
+            })
+        .finallyDo(
+            () -> {
+              /** Resets to 0 when inactive */
+              setClimbHeight(Meters.of(0));
+            });
+  }
+  /** Sets the climber io to idle */
+  public Command idleCommand() {
+    return Commands.runOnce(
+        () -> {
+          io.idle();
+        },
+        this);
+  }
+
+  public void configTestControls(Controller controller) {
+    controller.createBButton().whileTrue(climberCommand(Meters.of(.5)));
+  }
+
+  public void setClimbHeight(Distance height) {
+    io.setHeight(height);
+  }
+
+  public Distance getHeight() {
+    return inputs.climbHeight;
+  }
+
+  @Override
+  public void periodic() {
+    super.periodic();
+    io.updateInputs(inputs);
+    Logger.processInputs("Climb", inputs);
+  }
+
+  public void runClimb(double speed) {
+    io.runClimb(speed);
+  }
+
+  public boolean isRequested(ClimbState state) {
+    return inputs.stateRequested == state;
+  }
+
+  public boolean isCurrent(ClimbState state) {
+    return inputs.stateCurrent == state;
+  }
+
+  public void setCurrentState(ClimbState state) {
+    inputs.stateCurrent = state;
+  }
+
+  public void setRequestedState(ClimbState state) {
+    inputs.stateRequested = state;
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIO.java b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIO.java
new file mode 100644
index 00000000..6d284de0
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIO.java
@@ -0,0 +1,26 @@
+package frc.robot.rebuilt.subsystems.Climb;
+
+import static edu.wpi.first.units.Units.Inches;
+
+import edu.wpi.first.units.measure.Distance;
+import frc.robot.rebuilt.commands.ClimbCommands;
+import org.littletonrobotics.junction.AutoLog;
+
+/** IO interface for the Launcher subsystem. */
+public interface ClimbIO {
+
+  @AutoLog
+  public static class ClimbIOInputs {
+    public Distance climbHeight = Inches.of(0);
+    public ClimbCommands.ClimbState stateRequested = ClimbCommands.ClimbState.DISABLED;
+    public ClimbCommands.ClimbState stateCurrent = ClimbCommands.ClimbState.DISABLED;
+  }
+
+  public void runClimb(double speed);
+
+  public void idle();
+
+  public void setHeight(Distance height);
+
+  public default void updateInputs(ClimbIOInputs inputs) {}
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOReal.java b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOReal.java
new file mode 100644
index 00000000..f3fcb193
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOReal.java
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.Climb;
+
+import edu.wpi.first.units.measure.Distance;
+import java.util.Map;
+import yams.mechanisms.positional.Elevator;
+
+/** Add your docs here. */
+public class ClimbIOReal implements ClimbIO {
+  private static Elevator climber;
+
+  protected Map<String, Object> devices;
+
+  public ClimbIOReal(Map<String, Object> devices) {
+    this.devices = devices;
+
+    climber = (Elevator) devices.get("lifter");
+  }
+
+  @Override
+  public void updateInputs(ClimbIOInputs inputs) {
+
+    inputs.climbHeight = climber.getHeight();
+  }
+
+  public void idle() {
+    climber.getMotorController().setDutyCycle(0);
+  }
+
+  public void setHeight(Distance height) {
+    climber.getMotorController().setPosition(height);
+  }
+
+  public void runClimb(double speed) {
+    climber.getMotorController().setDutyCycle(speed);
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOSim.java b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOSim.java
new file mode 100644
index 00000000..b1cfa642
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOSim.java
@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.Climb;
+
+import java.util.Map;
+
+/** Add your docs here. */
+public class ClimbIOSim extends ClimbIOReal {
+
+  protected Map<String, Object> devices;
+
+  public ClimbIOSim(Map<String, Object> devices) {
+    super(devices);
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatus.java b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatus.java
new file mode 100644
index 00000000..4e567796
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatus.java
@@ -0,0 +1,26 @@
+package frc.robot.rebuilt.subsystems.DriverDisplay;
+
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.sensors.Controller;
+import org.littletonrobotics.junction.Logger;
+
+public class HubStatus extends GenericSubsystem {
+  private HubStatusIO io = new HubStatusIOImpl();
+  private HubStatusIOInputsAutoLogged inputs = new HubStatusIOInputsAutoLogged();
+
+  public void configureButtonBindings(Controller driver, Controller operator) {
+    // Trigger rumble = new Trigger(() -> inputs.timeRemainingInCurrentShift.lte(Seconds.of(3)));
+    // rumble.and(() -> inputs.timeRemainingInCurrentShift.gt(Seconds.of(2.5)));
+    // rumble
+    //     .onTrue(Commands.runOnce(() -> driver.setRumble(0.5)))
+    //     .onFalse(Commands.runOnce(() -> driver.setRumble(0)));
+  }
+
+  @Override
+  public void periodic() {
+    super.periodic();
+    io.updateInputs(inputs);
+
+    Logger.processInputs("HubStatus", inputs);
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIO.java b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIO.java
new file mode 100644
index 00000000..4b390d96
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIO.java
@@ -0,0 +1,20 @@
+package frc.robot.rebuilt.subsystems.DriverDisplay;
+
+import edu.wpi.first.units.measure.Time;
+import frc.robot.rebuilt.HubTracker.Shift;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface HubStatusIO {
+  @AutoLog
+  public static class HubStatusIOInputs {
+    public boolean activeNow = false;
+    Shift currentShift;
+    Time timeRemainingInCurrentShift;
+    Shift nextShift;
+    boolean isActiveNext;
+    String autoWinner;
+    double matchTime;
+  }
+
+  public void updateInputs(HubStatusIOInputs Inputs);
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIOImpl.java b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIOImpl.java
new file mode 100644
index 00000000..d40d1a4a
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIOImpl.java
@@ -0,0 +1,22 @@
+package frc.robot.rebuilt.subsystems.DriverDisplay;
+
+import static edu.wpi.first.units.Units.Seconds;
+
+import frc.robot.rebuilt.HubTracker;
+import frc.robot.rebuilt.HubTracker.Shift;
+
+public class HubStatusIOImpl implements HubStatusIO {
+
+  @Override
+  public void updateInputs(HubStatusIOInputs inputs) {
+    inputs.activeNow = HubTracker.isActive();
+
+    inputs.currentShift = HubTracker.getCurrentShift().orElse(Shift.AUTO);
+    inputs.timeRemainingInCurrentShift =
+        HubTracker.timeRemainingInCurrentShift().orElse(Seconds.of(0));
+    inputs.nextShift = HubTracker.getNextShift().orElse(Shift.AUTO);
+    inputs.isActiveNext = HubTracker.isActiveNext();
+    inputs.autoWinner = HubTracker.getAutoWinner().map(it -> it.toString()).orElse("NA");
+    inputs.matchTime = HubTracker.getMatchTime();
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/Indexer.java
new file mode 100644
index 00000000..77372997
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/Indexer.java
@@ -0,0 +1,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.Indexer;
+
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.commands.IndexerCommands.IndexerState;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.arch.StateMachine;
+import org.frc5010.common.sensors.Controller;
+import org.littletonrobotics.junction.Logger;
+
+public class Indexer extends GenericSubsystem {
+  private final IndexerIO io;
+  private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged();
+
+  /** Creates a new Index and selects the IO to real or simulated. */
+  public Indexer() {
+    super("indexer.json");
+
+    if (RobotBase.isSimulation()) {
+      io = (IndexerIO) new IndexerIOSim(devices);
+    } else {
+      io = new IndexerIOReal(devices);
+    }
+  }
+
+  public void runSpindexer(double speed) {
+    io.runSpindexer(speed);
+  }
+
+  public void runFeeder(double speed) {
+    io.runTransferFront(speed);
+  }
+
+  // public void runTransferBack(double speed) {
+  //   io.runTransferBack(speed);
+  // }
+
+  public void runTransferFront(double speed) {
+    io.runTransferFront(speed);
+  }
+
+  public void configTestControls(Controller controller) {
+    controller.createLeftBumper().whileTrue((spindexerCommand(.25)).alongWith(feederCommand(0.25)));
+  }
+  /** Command that runs the feeder at a given speed and stops when done */
+  public Command feederCommand(double speed) {
+    return Commands.run(
+            () -> {
+              runFeeder(0.25);
+            },
+            this)
+        .finallyDo(
+            () -> {
+              runFeeder(0);
+            });
+  }
+  /** returns a command that runs the spindexer at a set speed and stops when done */
+  public Command spindexerCommand(double speed) {
+    return Commands.run(
+            () -> {
+              runSpindexer(speed);
+            })
+        .finallyDo(
+            () -> {
+              runSpindexer(0);
+            });
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    super.periodic();
+    io.updateInputs(inputs);
+    Logger.processInputs("Indexer", inputs);
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    super.simulationPeriodic();
+  }
+
+  public boolean isRequested(IndexerState state) {
+    return inputs.stateRequested.compareTo(state) == 0;
+  }
+
+  public boolean isCurrent(IndexerState state) {
+    return inputs.stateCurrent.compareTo(state) == 0;
+  }
+
+  public void setCurrentState(IndexerState state) {
+    inputs.stateCurrent = state;
+  }
+
+  public void setRequestedState(IndexerState state) {
+    inputs.stateRequested = state;
+  }
+
+  public void setDefaultCommands(StateMachine stateMachine) {
+    inputs.stateRequested = IndexerState.IDLE;
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIO.java b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIO.java
new file mode 100644
index 00000000..2719e9d3
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIO.java
@@ -0,0 +1,23 @@
+package frc.robot.rebuilt.subsystems.Indexer;
+
+import frc.robot.rebuilt.commands.IndexerCommands;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IndexerIO {
+  @AutoLog
+  public static class IndexerIOInputs {
+    public double spindexerSpeed = 0;
+    public double transferFrontSpeed = 0;
+    public double transferBackSpeed = 0;
+    public IndexerCommands.IndexerState stateRequested = IndexerCommands.IndexerState.IDLE;
+    public IndexerCommands.IndexerState stateCurrent = IndexerCommands.IndexerState.IDLE;
+  }
+
+  public void runSpindexer(double speed);
+
+  public void runTransferFront(double speed);
+
+  // public void runTransferBack(double speed);
+
+  public default void updateInputs(IndexerIOInputs inputs) {}
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOReal.java b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOReal.java
new file mode 100644
index 00000000..02ec5b42
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOReal.java
@@ -0,0 +1,50 @@
+package frc.robot.rebuilt.subsystems.Indexer;
+
+import static edu.wpi.first.units.Units.Amps;
+
+import java.util.Map;
+import org.frc5010.common.motors.function.PercentControlMotor;
+import yams.mechanisms.velocity.FlyWheel;
+
+/** Implements the hardware Indexer IO */
+public class IndexerIOReal implements IndexerIO {
+  protected Map<String, Object> devices;
+  private PercentControlMotor spindexer;
+  // private PercentControlMotor transferFront, transferBack;
+  private FlyWheel transferFront;
+
+  public IndexerIOReal(Map<String, Object> devices) {
+    spindexer = (PercentControlMotor) devices.get("spindexer");
+    transferFront = (FlyWheel) devices.get("transfer");
+    spindexer.setCurrentLimit(Amps.of(120));
+    // transferFront = (PercentControlMotor) devicess.get("transfer_front");
+    // transferBack = (PercentControlMotor) devices.get("transfer_back");
+    // transferFront.invert(true);
+    // transferFront.setFollow(transferBack, false);
+    this.devices = devices;
+  }
+  /** Updates indexer input values with current motor speed */
+  @Override
+  public void updateInputs(IndexerIOInputs inputs) {
+    inputs.spindexerSpeed = spindexer.get();
+    inputs.transferFrontSpeed = transferFront.getMotor().getDutyCycle();
+    // inputs.transferFrontSpeed = transferFront.get();
+    // inputs.transferBackSpeed = transferBack.get();
+  }
+  /** Sets the spindexer motor speed */
+  @Override
+  public void runSpindexer(double speed) {
+    spindexer.set(speed);
+  }
+  /** Sets the front transfer motor speed */
+  @Override
+  public void runTransferFront(double speed) {
+    // transferFront.set(speed);
+    transferFront.getMotor().setDutyCycle(speed);
+  }
+
+  // @Override
+  // public void runTransferBack(double speed) {
+  //   transferBack.set(speed);
+  // }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOSim.java b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOSim.java
new file mode 100644
index 00000000..4e744926
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOSim.java
@@ -0,0 +1,11 @@
+package frc.robot.rebuilt.subsystems.Indexer;
+
+import java.util.Map;
+
+public class IndexerIOSim extends IndexerIOReal {
+  protected Map<String, Object> devices;
+
+  public IndexerIOSim(Map<String, Object> devices) {
+    super(devices);
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/FieldRegions.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/FieldRegions.java
new file mode 100644
index 00000000..466e6c6a
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/FieldRegions.java
@@ -0,0 +1,161 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.Launcher;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.rebuilt.FieldConstants;
+import java.util.Optional;
+import org.frc5010.common.utils.geometry.AllianceFlipUtil;
+
+/** Add your docs here. */
+public class FieldRegions {
+  static double topTrenchLeftX = FieldConstants.TrenchZoneTop.nearAllianceLeftDanger.getX();
+  static double topTrenchRightX = FieldConstants.TrenchZoneTop.nearAllianceRightDanger.getX();
+
+  static double topTrenchY = FieldConstants.TrenchZoneTop.nearAllianceLeftDanger.getY();
+
+  static double topOppTrenchLeftX = FieldConstants.TrenchZoneTop.oppAllianceLeftDanger.getX();
+  static double topOppTrenchRightX = FieldConstants.TrenchZoneTop.oppAllianceRightDanger.getX();
+
+  static double lowerTrenchLeftX = FieldConstants.TrenchZoneBottom.nearAllianceLeftDanger.getX();
+  static double lowerTrenchRightX = FieldConstants.TrenchZoneBottom.nearAllianceRightDanger.getX();
+  static double lowerTrenchY = FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger.getY();
+
+  static double lowerOppTrenchLeftX = FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger.getX();
+  static double lowerOppTrenchRightX =
+      FieldConstants.TrenchZoneBottom.oppAllianceRightDanger.getX();
+
+  static Translation2d allianceCornerOrigin = new Translation2d(0, 0);
+  static Translation2d AllianceCornerTrench =
+      new Translation2d(
+          FieldConstants.TrenchZoneBottom.nearAlliance.getX()
+              - 1 / 2 * FieldConstants.LeftTrench.depth,
+          FieldConstants.fieldWidth);
+  static Translation2d topRightMidTrenchCorner =
+      new Translation2d(
+          FieldConstants.TrenchZoneTop.oppAlliance.getX()
+              - 1 / 2 * FieldConstants.RightTrench.depth,
+          FieldConstants.fieldWidth);
+  static Translation2d bottomRightMidTrenchCorner =
+      new Translation2d(
+          FieldConstants.TrenchZoneTop.oppAlliance.getX() - 1 / 2 * FieldConstants.LeftTrench.depth,
+          0);
+  static Translation2d oppTopRightOrigin =
+      new Translation2d(FieldConstants.fieldLength, FieldConstants.fieldWidth);
+  static Translation2d oppBottemRightOrigin = new Translation2d(FieldConstants.fieldLength, 0);
+
+  static Rectangle2d allianceField = new Rectangle2d(allianceCornerOrigin, AllianceCornerTrench);
+  static Rectangle2d upperMidField =
+      new Rectangle2d(FieldConstants.Hub.farLeftCorner, topRightMidTrenchCorner);
+  static Rectangle2d lowerMidField =
+      new Rectangle2d(FieldConstants.Hub.farRightCorner, bottomRightMidTrenchCorner);
+  static Rectangle2d oppUpperField =
+      new Rectangle2d(FieldConstants.Hub.oppFarLeftCorner, oppTopRightOrigin);
+  static Rectangle2d oppLowerField =
+      new Rectangle2d(FieldConstants.Hub.oppFarRightCorner, oppBottemRightOrigin);
+
+  public static void setupFieldRegions() {
+    topTrenchLeftX =
+        AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.nearAllianceLeftDanger).getX();
+    topTrenchRightX =
+        AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.nearAllianceRightDanger).getX();
+    topTrenchY = AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.nearAllianceLeftDanger).getY();
+    topOppTrenchLeftX =
+        AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.oppAllianceLeftDanger).getX();
+    topOppTrenchRightX =
+        AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.oppAllianceRightDanger).getX();
+    lowerTrenchLeftX =
+        AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.nearAllianceLeftDanger).getX();
+    lowerTrenchRightX =
+        AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.nearAllianceRightDanger).getX();
+    lowerTrenchY =
+        AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger).getY();
+    lowerOppTrenchLeftX =
+        AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger).getX();
+    lowerOppTrenchRightX =
+        AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.oppAllianceRightDanger).getX();
+
+    allianceCornerOrigin = AllianceFlipUtil.apply(new Translation2d(0, 0));
+    AllianceCornerTrench =
+        AllianceFlipUtil.apply(
+            new Translation2d(
+                FieldConstants.TrenchZoneBottom.nearAlliance.getX()
+                    - 1 / 2 * FieldConstants.LeftTrench.depth,
+                FieldConstants.fieldWidth));
+    topRightMidTrenchCorner =
+        AllianceFlipUtil.apply(
+            new Translation2d(
+                FieldConstants.TrenchZoneTop.oppAlliance.getX()
+                    - 1 / 2 * FieldConstants.RightTrench.depth,
+                FieldConstants.fieldWidth));
+    bottomRightMidTrenchCorner =
+        AllianceFlipUtil.apply(
+            new Translation2d(
+                FieldConstants.TrenchZoneTop.oppAlliance.getX()
+                    - 1 / 2 * FieldConstants.LeftTrench.depth,
+                0));
+    oppTopRightOrigin =
+        AllianceFlipUtil.apply(
+            new Translation2d(FieldConstants.fieldLength, FieldConstants.fieldWidth));
+    oppBottemRightOrigin = AllianceFlipUtil.apply(new Translation2d(FieldConstants.fieldLength, 0));
+
+    allianceField = new Rectangle2d(allianceCornerOrigin, AllianceCornerTrench);
+    upperMidField = new Rectangle2d(FieldConstants.Hub.farLeftCorner, topRightMidTrenchCorner);
+    lowerMidField = new Rectangle2d(FieldConstants.Hub.farRightCorner, bottomRightMidTrenchCorner);
+    oppUpperField = new Rectangle2d(FieldConstants.Hub.oppFarLeftCorner, oppTopRightOrigin);
+    oppLowerField = new Rectangle2d(FieldConstants.Hub.oppFarRightCorner, oppBottemRightOrigin);
+  }
+
+  public static boolean isNearTrench(double currentX, double currentY) {
+    boolean nearAllianceTop =
+        ((currentX > topTrenchLeftX && currentX < topTrenchRightX) && currentY > topTrenchY);
+
+    boolean nearOppAllianceTop =
+        ((currentX > topOppTrenchLeftX && currentX < topOppTrenchRightX) && currentY > topTrenchY);
+
+    boolean nearAllianceBottom =
+        ((currentX > lowerTrenchLeftX && currentX < lowerTrenchRightX) && currentY < lowerTrenchY);
+
+    boolean nearOppAllianceBottom =
+        ((currentX > lowerOppTrenchLeftX && currentX < lowerOppTrenchRightX)
+            && currentY < lowerTrenchY);
+
+    return nearAllianceTop || nearOppAllianceTop || nearAllianceBottom || nearOppAllianceBottom;
+  }
+
+  static Translation2d leftAdjustment = new Translation2d(Meters.of(1), Meters.of(1.5));
+  static Translation2d rightAdjustment = new Translation2d(Meters.of(1), Meters.of(-1.5));
+
+  public static Optional<Translation2d> determineTargetPose(Pose2d currentPose) {
+    Boolean inAllianceField = allianceField.contains(currentPose.getTranslation());
+    Boolean inUpperMidField = upperMidField.contains(currentPose.getTranslation());
+    Boolean inLowerMidField = lowerMidField.contains(currentPose.getTranslation());
+    Boolean inOppUpperField = oppUpperField.contains(currentPose.getTranslation());
+    Boolean inOppLowerField = oppLowerField.contains(currentPose.getTranslation());
+    SmartDashboard.putBoolean("In Alliance Field", inAllianceField);
+    SmartDashboard.putBoolean("In Upper Mid Field", inUpperMidField);
+    SmartDashboard.putBoolean("In Lower Mid Field", inLowerMidField);
+    SmartDashboard.putBoolean("In Opp Upper Field", inOppUpperField);
+    SmartDashboard.putBoolean("In Opp Lower Field", inOppLowerField);
+    if (inAllianceField) {
+      return Optional.of(FieldConstants.Hub.topCenterPoint.toTranslation2d());
+    } else if (inUpperMidField) {
+      return Optional.of(FieldConstants.Tower.leftUpright.plus(leftAdjustment));
+    } else if (inLowerMidField) {
+      return Optional.of(FieldConstants.Tower.rightUpright.plus(rightAdjustment));
+    } else if (inOppUpperField) {
+      return Optional.of(FieldConstants.Tower.leftUpright.plus(leftAdjustment));
+    } else if (inOppLowerField) {
+      return Optional.of(FieldConstants.Tower.rightUpright.plus(rightAdjustment));
+    } else {
+      return Optional.empty();
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/Launcher.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/Launcher.java
new file mode 100644
index 00000000..97504199
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/Launcher.java
@@ -0,0 +1,418 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.Launcher;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.RPM;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.Constants;
+import frc.robot.rebuilt.Rebuilt;
+import frc.robot.rebuilt.commands.LauncherCommands.LauncherState;
+import java.util.Map;
+import java.util.function.Supplier;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.littletonrobotics.junction.Logger;
+import yams.mechanisms.positional.Arm;
+import yams.mechanisms.positional.Pivot;
+
+public class Launcher extends GenericSubsystem {
+  private final LauncherIO io;
+  private final Arm hood;
+  private final LauncherIOInputsAutoLogged inputs = new LauncherIOInputsAutoLogged();
+  public static Transform3d robotToTurret = new Transform3d();
+  private Map<String, GenericSubsystem> subsystems;
+  private SmartTurretController smartTurretController;
+
+  private static final double PROFILE_PERIOD_SECONDS = 0.005; // 200 Hz
+
+  /** Creates a new Launcher. */
+  public Launcher(Map<String, GenericSubsystem> subsystems) {
+    super("launcher.json");
+
+    this.subsystems = subsystems;
+    Pivot turret = (Pivot) devices.get("turret");
+    hood = (Arm) devices.get("hood");
+    robotToTurret =
+        new Transform3d(
+            turret.getPivotConfig().getMechanismPositionConfig().getRelativePosition().get(),
+            new Rotation3d());
+    /** Chooses the IO implimentation to be real or simulated */
+    if (RobotBase.isSimulation()) {
+      io = new LauncherIOSim(devices, subsystems);
+    } else {
+      io = new LauncherIOReal(devices, subsystems);
+    }
+
+    io.configureShotCalculator(ShotCalculator.getInstance());
+
+    // Register the SmartTurretController's high-frequency stepping loop.
+    // This runs at 200 Hz (5 ms) via a Notifier, evaluating state transitions and
+    // sending control requests to the TalonFX.
+    smartTurretController = io.getSmartTurretController();
+    if (smartTurretController != null) {
+      Notifier profileNotifier =
+          new Notifier(() -> smartTurretController.step(PROFILE_PERIOD_SECONDS));
+      profileNotifier.setName("SmartTurret");
+      profileNotifier.startPeriodic(PROFILE_PERIOD_SECONDS);
+    }
+  }
+
+  /**
+   * Updates the inputs of the Launcher subsystem from the physical devices.
+   *
+   * <p>This method is called periodically by the GenericSubsystem class.
+   */
+  @Override
+  public void periodic() {
+    super.periodic();
+
+    io.updateInputs(inputs);
+    Logger.processInputs("Launcher", inputs);
+  }
+
+  /**
+   * Called every time the scheduler runs while the robot is in simulation mode. Used to update
+   * simulation models.
+   */
+  @Override
+  public void simulationPeriodic() {
+    super.simulationPeriodic();
+    io.updateSimulation(this, Rebuilt.indexer);
+  }
+
+  /**
+   * Run the shooter at the given speed. This is a convenience method which simply calls
+   * setUpperSpeed with the given speed.
+   *
+   * @param speed the speed to set the upper shooter motor to, in units of RPM.
+   */
+  public void runShooter(double speed) {
+    io.runShooter(speed);
+  }
+
+  public void setHoodAngle(Angle angle) {
+    io.setHoodAngle(angle);
+  }
+
+  public void setTurretRotation(Angle angle) {
+    io.setTurretRotation(angle);
+  }
+
+  public boolean isShooting() {
+    return inputs.stateCurrent == LauncherState.PREP || inputs.stateCurrent == LauncherState.PRESET;
+  }
+
+  public Command getHoodSysIdCommand() {
+    return io.getHoodSysIdCommand(this);
+  }
+
+  public Command getTurretSysIdCommand() {
+    return io.getTurretSysIdCommand(this);
+  }
+
+  public Command getFlyWheelSysIdCommand() {
+    return io.getFlyWheelSysIdCommand(this);
+  }
+
+  public Command getHoodCharacterizationCommand() {
+    return io.getHoodCharacterizationCommand(this);
+  }
+
+  public Command getTurretCharacterizationCommand() {
+    return io.getTurretCharacterizationCommand(this);
+  }
+
+  public Command getTurretQuasistaticCommand() {
+    return io.getTurretQuasistaticCommand(this);
+  }
+
+  public Command getTurretDynamicCommand() {
+    return io.getTurretDynamicCommand(this);
+  }
+
+  public Command getTurretKsMapCommand() {
+    return io.getTurretKsMapCommand(this);
+  }
+
+  public Command getTurretTrackingTuneCommand() {
+    return io.getTurretTrackingTuneCommand(this);
+  }
+
+  public Command getTurretSeekingTuneCommand() {
+    return io.getTurretSeekingTuneCommand(this);
+  }
+
+  public Translation2d getRobotTarget() {
+    return io.determineTarget().get();
+  }
+
+  @Override
+  public Command getDefaultCommand() {
+    return Commands.runOnce(
+        () -> {
+          io.stopAllMotors();
+        },
+        this);
+  }
+
+  public void stopAllMotors() {
+    io.stopAllMotors();
+  }
+  /** Command that aims the launcher using hood turret and flywheel values from calculations */
+  public Command trackTargetCommand() {
+    return Commands.run(
+        () -> {
+          io.setHoodAngle(inputs.hoodAngleCalculated);
+          io.setTurretRotationWithFeedforward(
+              inputs.turretAngleCalculated,
+              inputs.turretFeedforwardRadPerSec,
+              inputs.turretFeedforwardAccelRadPerSecSq);
+          io.setFlyWheelVelocity(inputs.flyWheelSpeedCalculated);
+        });
+  }
+  /** Aims the launcher using the preset hood angle and calculates flywheel and turret values */
+  public Command trackTargetLowCommand() {
+    return Commands.run(
+        () -> {
+          io.setHoodAngleLow();
+          io.setTurretRotationWithFeedforward(
+              inputs.turretAngleCalculated,
+              inputs.turretFeedforwardRadPerSec,
+              inputs.turretFeedforwardAccelRadPerSecSq);
+          io.setFlyWheelVelocity(inputs.flyWheelSpeedCalculated);
+        });
+  }
+  /** Aims the turret and sets the flywheel to a given speed */
+  public Command trackTargetCommand(double speed) {
+    return Commands.run(
+        () -> {
+          io.setHoodAngle(hood.getMotorController().getConfig().getMechanismLowerLimit().get());
+          io.setTurretRotationWithFeedforward(
+              inputs.turretAngleCalculated,
+              inputs.turretFeedforwardRadPerSec,
+              inputs.turretFeedforwardAccelRadPerSecSq);
+          io.setFlyWheelVelocity(RPM.of(speed));
+        });
+  }
+
+  /**
+   * A command which stops the tracking of a target and resets the turret rotation and hood angle to
+   * 0 degrees.
+   *
+   * @return a command which stops tracking and resets the turret rotation and hood angle.
+   */
+  public Command stopTrackingCommand() {
+    return Commands.runOnce(
+        () -> {
+          setTurretRotation(Degrees.of(0));
+          setHoodAngle(Constants.Launcher.LOW_HOOD_ANGLE);
+          io.setFlyWheelVelocity(RPM.of(0));
+        });
+  }
+
+  /**
+   * Checks if the robot is at the desired speed and angle. This method returns true if the robot
+   * speed and angle are within the allowed tolerances of the desired values.
+   *
+   * @return true if the robot is at the desired speed and angle, false otherwise.
+   */
+  public boolean isAtGoal() {
+    return inputs.flyWheelSpeedAtGoal
+        && inputs.hoodAngleAtGoal
+        && inputs.turretAngleAtGoal
+        && inputs.isValidCalculation;
+  }
+
+  public boolean isOKToFire() {
+    return inputs.isValidCalculation;
+  }
+
+  public boolean isRequested(LauncherState state) {
+    return inputs.stateRequested == state;
+  }
+  /** Checks if the current launcher matches the given state */
+  public boolean isCurrent(LauncherState state) {
+    return inputs.stateCurrent == state;
+  }
+  /** Updates the launcher's current state */
+  public void setCurrentState(LauncherState state) {
+    inputs.stateCurrent = state;
+  }
+  /** Sets the launcher's requested state to transition to */
+  public void setRequestedState(LauncherState state) {
+    inputs.stateRequested = state;
+  }
+
+  public LauncherState getCurrentState() {
+    return inputs.stateCurrent;
+  }
+
+  public LauncherState getPreTrenchState() {
+    return inputs.preTrenchState;
+  }
+
+  public boolean isNearTrench() {
+    boolean nearTrench = io.isNearTrench();
+    if (nearTrench && (getCurrentState() != LauncherState.AUTO_HAMMERTIME)) {
+      inputs.preTrenchState = getCurrentState();
+      if (getCurrentState() == LauncherState.PRESET) {
+        inputs.preTrenchState = LauncherState.LOW_SPEED;
+      }
+    }
+    return nearTrench;
+  }
+
+  /** Applies the hood and turret angle, and the flywheel speed */
+  public void usePresets(Angle hoodAngle, Angle turretAngle, AngularVelocity flywheelSpeed) {
+    io.setHoodAngle(hoodAngle);
+    io.setTurretRotation(turretAngle);
+    io.setFlyWheelVelocity(flywheelSpeed);
+  }
+
+  public ShotCalculator.ShootingParameters getShootingParameters(
+      Supplier<Pose2d> robotPoseSupplier, Supplier<Translation2d> targetPositionSupplier) {
+    return io.getShootingParameters(robotPoseSupplier, targetPositionSupplier);
+  }
+
+  // ---- Actual (measured) value getters for tuning/telemetry ----
+
+  /** Get the actual hood angle as measured by the encoder. */
+  public Angle getHoodAngleActual() {
+    return inputs.hoodAngleActual;
+  }
+
+  /** Get the actual turret angle as measured by the encoder. */
+  public Angle getTurretAngleActual() {
+    return inputs.turretAngleActual;
+  }
+
+  /** Get the actual flywheel speed as measured by the encoder. */
+  public AngularVelocity getFlywheelSpeedActual() {
+    return inputs.flyWheelSpeedActual;
+  }
+
+  // ---- Desired (setpoint) value getters for tuning/telemetry ----
+
+  /** Get the desired hood angle setpoint. */
+  public Angle getHoodAngleDesired() {
+    return inputs.hoodAngleDesired;
+  }
+
+  /** Get the desired turret angle setpoint. */
+  public Angle getTurretAngleDesired() {
+    return inputs.turretAngleDesired;
+  }
+
+  /** Get the desired flywheel speed setpoint. */
+  public AngularVelocity getFlywheelSpeedDesired() {
+    return inputs.flyWheelSpeedDesired;
+  }
+
+  // ---- Error and at-goal getters for tuning/telemetry ----
+
+  /** Get the flywheel speed error (actual - desired). */
+  public AngularVelocity getFlywheelSpeedError() {
+    return inputs.flyWheelSpeedError;
+  }
+
+  /** Get the hood angle error in degrees (actual - desired). */
+  public double getHoodAngleError() {
+    return inputs.hoodAngleError;
+  }
+
+  /** Get the turret angle error in degrees (actual - desired). */
+  public double getTurretAngleError() {
+    return inputs.turretAngleError;
+  }
+
+  /** Whether the flywheel speed is within tolerance of the setpoint. */
+  public boolean isFlywheelAtGoal() {
+    return inputs.flyWheelSpeedAtGoal;
+  }
+
+  public boolean isFlywheelAtOrAboveGoal() {
+    return inputs.flyWheelSpeedAtGoal || inputs.flyWheelSpeedActual.gt(inputs.flyWheelSpeedDesired);
+  }
+
+  /** Whether the hood angle is within tolerance of the setpoint. */
+  public boolean isHoodAtGoal() {
+    return inputs.hoodAngleAtGoal;
+  }
+
+  /** Whether the turret angle is within tolerance of the setpoint. */
+  public boolean isTurretAtGoal() {
+    return inputs.turretAngleAtGoal;
+  }
+
+  public Command increaseHoodAngleCommand() {
+    return Commands.runOnce(
+        () -> {
+          Angle newAngle = inputs.hoodAngleActual.plus(Degrees.of(0.5));
+          if (newAngle.lt(Degrees.of(60))) {
+            io.setHoodAngle(newAngle);
+          }
+        });
+  }
+  /** Decreases the hood angle by 0.5 degrees and ensures it does not go below 30 degrees */
+  public Command decreaseHoodAngleCommand() {
+    return Commands.runOnce(
+        () -> {
+          Angle newAngle = inputs.hoodAngleActual.minus(Degrees.of(0.5));
+          if (newAngle.gt(Degrees.of(30))) {
+            io.setHoodAngle(newAngle);
+          }
+        });
+  }
+  /** Decreases the flywheel speed by 10 RPM and ensures it does not go below 0 RPM */
+  public Command decreaseFlywheelSpeedCommand() {
+    return Commands.runOnce(
+        () -> {
+          AngularVelocity newSpeed = inputs.flyWheelSpeedActual.minus(RPM.of(10));
+          if (newSpeed.gt(RPM.of(0))) {
+            io.setFlyWheelVelocity(newSpeed);
+          }
+        });
+  }
+  /** Increases the flywheel speed by 10 RPM and ensures it does not go above 300 RPM */
+  public Command increaseFlywheelSpeedCommand() {
+    return Commands.runOnce(
+        () -> {
+          AngularVelocity newSpeed = inputs.flyWheelSpeedActual.plus(RPM.of(10));
+          if (newSpeed.lt(RPM.of(300))) { // Assuming 300 RPM as the upper limit
+            io.setFlyWheelVelocity(newSpeed);
+          }
+        });
+  }
+  /** Decreases the turret angle by 10 degrees and ensures it does not go below -90 */
+  public Command decreaseTurretAngleCommand() {
+    return Commands.runOnce(
+        () -> {
+          Angle newAngle = inputs.turretAngleActual.minus(Degrees.of(1));
+          if (newAngle.gt(Degrees.of(-90))) { // Assuming -90 degrees as the left limit
+            io.setTurretRotation(newAngle);
+          }
+        });
+  }
+  /** Increases the turret angle by 1 degree and ensures it does not go above 90 degrees */
+  public Command increaseTurretAngleCommand() {
+    return Commands.runOnce(
+        () -> {
+          Angle newAngle = inputs.turretAngleActual.plus(Degrees.of(1));
+          if (newAngle.lt(Degrees.of(90))) { // Assuming 90 degrees as the right limit
+            io.setTurretRotation(newAngle);
+          }
+        });
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIO.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIO.java
new file mode 100644
index 00000000..5a4d2ccf
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIO.java
@@ -0,0 +1,167 @@
+package frc.robot.rebuilt.subsystems.Launcher;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.commands.LauncherCommands;
+import frc.robot.rebuilt.subsystems.Indexer.Indexer;
+import java.util.Optional;
+import java.util.function.Supplier;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.littletonrobotics.junction.AutoLog;
+
+/** IO interface for the Launcher subsystem. */
+public interface LauncherIO {
+
+  @AutoLog
+  public static class LauncherIOInputs {
+    /** Initializes the requested and current launcher states to idle */
+    public LauncherCommands.LauncherState stateRequested = LauncherCommands.LauncherState.IDLE;
+
+    public LauncherCommands.LauncherState stateCurrent = LauncherCommands.LauncherState.IDLE;
+    public LauncherCommands.LauncherState preTrenchState = LauncherCommands.LauncherState.IDLE;
+    /**
+     * Intializes the distance to the virtual target and desired flywheel speed to start at 0 and
+     * calculation validity to false
+     */
+    public boolean isValidCalculation = false;
+
+    public Distance distanceToVirtualTarget = Meters.of(0.0);
+    public AngularVelocity flyWheelSpeedDesired = RPM.of(0.0);
+    /**
+     * Intializes calculated and desired angles to 0 degrees and calculated flywheel speed to 0 RPS
+     */
+    public AngularVelocity flyWheelSpeedCalculated = RotationsPerSecond.of(0.0);
+
+    public Angle hoodAngleCalculated = Degrees.of(0.0);
+    public Angle turretAngleCalculated = Degrees.of(0.0);
+    public Angle hoodAngleDesired = Degrees.of(0.0);
+    public Angle turretAngleDesired = Degrees.of(0.0);
+    /** Initializes actual flywheel speed to 0 RPM and actual hood and turret angles to 0 degrees */
+    public AngularVelocity flyWheelSpeedActual = RPM.of(0.0);
+
+    public Angle hoodAngleActual = Degrees.of(0.0);
+    public Angle turretAngleActual = Degrees.of(0.0);
+    /** Decides whether the flywheel speed and turret and hood angle have reached their goals */
+    public boolean flyWheelSpeedAtGoal = false;
+
+    public boolean hoodAngleAtGoal = false;
+    public boolean turretAngleAtGoal = false;
+    /** Initializes the hood and turret angle errors to 0 and the flywheel speed error to 0 RPM */
+    public AngularVelocity flyWheelSpeedError = RPM.of(0.0);
+
+    public double hoodAngleError = 0.0;
+    public double turretAngleError = 0.0;
+    /** Intiializes the hood and turret velocities to 0 and the flywheel motor output to 0 */
+    public double hoodVelocity = 0.0;
+
+    public double turretVelocity = 0.0;
+    public double flyWheelMotorOutput = 0.0;
+
+    /** Kinematic feedforward for the turret (rad/s), computed via numerical differentiation. */
+    public double turretFeedforwardRadPerSec = 0.0;
+
+    /**
+     * Acceleration feedforward for the turret (rad/s^2), computed via numerical differentiation.
+     */
+    public double turretFeedforwardAccelRadPerSecSq = 0.0;
+
+    public Translation2d robotToTarget = new Translation2d();
+
+    public Distance targetDistance = Meters.of(0.0);
+
+    public Angle uniqueCoverage = Degrees.of(0.0);
+    public boolean coverageSatisfiesRange = false;
+  }
+
+  public default void updateInputs(LauncherIOInputs inputs) {}
+
+  public void runShooter(double speed);
+
+  public void setFlyWheelVelocity(AngularVelocity speed);
+
+  public void setHoodAngle(Angle angle);
+
+  public void setHoodAngleLow();
+
+  public void setTurretRotation(Angle angle);
+
+  /**
+   * Sets the turret to the given angle while simultaneously applying kinematic feedforward velocity
+   * and acceleration. The feedforward values are passed directly to the underlying motor
+   * controller's closed-loop request.
+   *
+   * @param angle desired turret mechanism angle
+   * @param feedforwardRadPerSec angular velocity feedforward in rad/s (mechanism units)
+   * @param accelerationRadPerSecSq angular acceleration feedforward in rad/s^2 (mechanism units)
+   */
+  public default void setTurretRotationWithFeedforward(
+      Angle angle, double feedforwardRadPerSec, double accelerationRadPerSecSq) {
+    setTurretRotation(angle);
+  }
+
+  public LinearVelocity getFlyWheelExitSpeed(AngularVelocity velocity);
+
+  public Command getHoodCharacterizationCommand(GenericSubsystem launcher);
+
+  public Command getHoodSysIdCommand();
+
+  public Command getHoodSysIdCommand(GenericSubsystem launcher);
+
+  public Command getTurretCharacterizationCommand(GenericSubsystem launcher);
+
+  public Command getTurretSysIdCommand();
+
+  public Command getFlyWheelSysIdCommand(GenericSubsystem launcher);
+
+  public Command getFlyWheelSysIdCommand();
+
+  public Command getTurretSysIdCommand(GenericSubsystem launcher);
+
+  public ShotCalculator.ShootingParameters getShootingParameters(
+      Supplier<Pose2d> robotPoseSupplier, Supplier<Translation2d> targetPositionSupplier);
+
+  public void stopAllMotors();
+
+  public default void configureShotCalculator(ShotCalculator shotCalculator) {}
+
+  public default SmartTurretController getSmartTurretController() {
+    return null;
+  }
+
+  public default void updateSimulation(Launcher launcher, Indexer indexer) {}
+
+  public boolean isNearTrench();
+
+  public Optional<Translation2d> determineTarget();
+
+  public default Command getTurretQuasistaticCommand(GenericSubsystem launcher) {
+    return Commands.none();
+  }
+
+  public default Command getTurretDynamicCommand(GenericSubsystem launcher) {
+    return Commands.none();
+  }
+
+  public default Command getTurretKsMapCommand(GenericSubsystem launcher) {
+    return Commands.none();
+  }
+
+  public default Command getTurretTrackingTuneCommand(GenericSubsystem launcher) {
+    return Commands.none();
+  }
+
+  public default Command getTurretSeekingTuneCommand(GenericSubsystem launcher) {
+    return Commands.none();
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOReal.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOReal.java
new file mode 100644
index 00000000..3ede542b
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOReal.java
@@ -0,0 +1,568 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.Launcher;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.Radian;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.hardware.CANcoder;
+import edu.wpi.first.math.controller.ArmFeedforward;
+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.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.rebuilt.Constants;
+import frc.robot.rebuilt.Rebuilt;
+import frc.robot.rebuilt.commands.IntakeCommands.IntakeState;
+import frc.robot.rebuilt.commands.LauncherCommands;
+import frc.robot.rebuilt.subsystems.intake.Intake;
+import java.util.Map;
+import java.util.Optional;
+import java.util.function.Supplier;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.config.ConfigConstants;
+import org.frc5010.common.drive.GenericDrivetrain;
+import org.frc5010.common.motors.SystemIdentification;
+import org.frc5010.common.subsystems.LEDStrip;
+import org.frc5010.common.vision.AprilTags;
+import yams.mechanisms.config.SensorConfig;
+import yams.mechanisms.positional.Arm;
+import yams.mechanisms.positional.Pivot;
+import yams.mechanisms.velocity.FlyWheel;
+import yams.motorcontrollers.simulation.Sensor;
+import yams.units.EasyCRT;
+import yams.units.EasyCRTConfig;
+
+/** Add your docs here. */
+public class LauncherIOReal implements LauncherIO {
+
+  protected Map<String, Object> devices;
+  protected Pivot turret;
+  protected Arm hood;
+  protected GenericDrivetrain drivetrain;
+  protected FlyWheel flyWheel;
+  protected CANcoder crtEncoder40;
+  protected CANcoder crtEncoder36;
+  protected final Sensor crtSensor40;
+  protected final Sensor crtSensor36;
+  protected EasyCRT easyCrtSolver;
+  /** Initializes the launcher hardware, encoders, simulated sensors, and angle solver */
+  EasyCRTConfig easyCrt;
+
+  private boolean isNearTrench = false;
+  private IntakeState lastState = IntakeState.RETRACTED;
+
+  protected Intake intake;
+
+  protected static Translation2d robotToTurret;
+
+  Angle turretLowLimit = Degrees.of(-90);
+  Angle turretHighLimit = Degrees.of(90);
+
+  /** 2-state turret controller: SEEKING (MotionMagic) and TRACKING (Position + FF). */
+  protected SmartTurretController smartTurretController;
+
+  /** Previous turret desired angle (rad) for numerical feedforward differentiation. */
+  private double previousTurretDesiredAngleRad = 0.0;
+
+  /** Previous turret velocity feedforward (rad/s) for numerical acceleration computation. */
+  private double previousTurretVelocityRadPerSec = 0.0;
+
+  public LauncherIOReal(Map<String, Object> devices, Map<String, GenericSubsystem> subsystems) {
+    this.devices = devices;
+    drivetrain = (GenericDrivetrain) subsystems.get(ConfigConstants.DRIVETRAIN);
+    intake = (Intake) subsystems.get(Constants.INTAKE);
+    turret = (Pivot) devices.get("turret");
+    robotToTurret =
+        turret
+            .getPivotConfig()
+            .getMechanismPositionConfig()
+            .getRelativePosition()
+            .get()
+            .toTranslation2d();
+
+    hood = (Arm) devices.get("hood");
+    flyWheel = (FlyWheel) devices.get("flywheel");
+
+    turretLowLimit =
+        turret.getMotorController().getConfig().getMechanismLowerLimit().orElse(turretLowLimit);
+    turretHighLimit =
+        turret.getMotorController().getConfig().getMechanismUpperLimit().orElse(turretHighLimit);
+
+    CANBus canivoreBus = new CANBus("canivore");
+    crtEncoder40 = new CANcoder(21, canivoreBus);
+    crtEncoder36 = new CANcoder(22, canivoreBus);
+    double sensor40Sim = 0.391;
+    double sensor36Sim = 0.274;
+    crtSensor40 =
+        new SensorConfig("CRT sensor 40")
+            .withField("angle", () -> crtEncoder40.getAbsolutePosition().getValueAsDouble(), 0.0)
+            .withSimulatedValue("angle", Seconds.of(0), Seconds.of(0.5), sensor40Sim)
+            .getSensor();
+    crtSensor36 =
+        new SensorConfig("CRT sensor 36")
+            .withField("angle", () -> crtEncoder36.getAbsolutePosition().getValueAsDouble(), 0.0)
+            .withSimulatedValue("angle", Seconds.of(0), Seconds.of(0.5), sensor36Sim)
+            .getSensor();
+
+    easyCrt =
+        new EasyCRTConfig(
+                () -> Rotations.of(crtSensor40.getAsDouble("angle")),
+                () -> Rotations.of(crtSensor36.getAsDouble("angle")))
+            .withCommonDriveGear(
+                /* commonRatio (mech:drive) */ 30.0,
+                /* driveGearTeeth */ 12,
+                /* encoder1Pinion */ 40,
+                /* encoder2Pinion */ 36)
+            .withAbsoluteEncoderOffsets( // -0.474609375
+                Rotations.of(-0.07470703125),
+                Rotations.of(-0.167236328125)) // set after mechanical zero
+            .withMechanismRange(Degrees.of(-168), Degrees.of(173)) // -360 deg to +720 deg
+            .withMatchTolerance(Rotations.of(0.06)) // ~1.08 deg at encoder2 for the example ratio
+            .withAbsoluteEncoderInversions(true, false)
+            .withCrtGearRecommendationConstraints(
+                /* coverageMargin */ 1.2,
+                /* minTeeth */ 15,
+                /* maxTeeth */ 45,
+                /* maxIterations */ 30);
+
+    easyCrtSolver = new EasyCRT(easyCrt);
+    // // Test Values
+    SmartDashboard.putNumber(
+        "EasyCRT/Unique Coverage", easyCrt.getUniqueCoverage().orElse(Degrees.of(0.0)).in(Degrees));
+    SmartDashboard.putBoolean("EasyCRT/Coverage Satisfies Range", easyCrt.coverageSatisfiesRange());
+    SmartDashboard.putNumber("EasyCRT/Enc 1", easyCrt.getAbsoluteEncoder1Angle().in(Degrees));
+    SmartDashboard.putNumber(
+        "EasyCRT/Enc 1 Ratio", easyCrt.getEncoder1RotationsPerMechanismRotation());
+    SmartDashboard.putNumber("EasyCRT/Enc 2", easyCrt.getAbsoluteEncoder2Angle().in(Degrees));
+    SmartDashboard.putNumber(
+        "EasyCRT/Enc 2 Ratio", easyCrt.getEncoder2RotationsPerMechanismRotation());
+    Angle calculatedAngle;
+    Optional<Angle> optionalAngle = (easyCrtSolver.getAngleOptional());
+    if (optionalAngle.isPresent()) {
+      calculatedAngle = optionalAngle.get();
+    } else {
+      calculatedAngle = Degrees.of(0);
+    }
+
+    SmartDashboard.putNumber("EasyCRT/CRT Angle", calculatedAngle.in(Degrees));
+    SmartDashboard.putString("EasyCRT/CRT Status", easyCrtSolver.getLastStatus().name());
+    SmartDashboard.putNumber("EasyCRT/CRT Error Rot", easyCrtSolver.getLastErrorRotations());
+    turret.getMotor().setEncoderPosition(calculatedAngle);
+
+    // Create the 2-state SmartTurretController (replaces TurretProfileController).
+    // Uses MotionMagicTorqueCurrentFOC for seeking and PositionTorqueCurrentFOC for tracking.
+    {
+      var turretConfig = turret.getMotorController().getConfig();
+      var trapConstraints = turretConfig.getTrapezoidProfile();
+      // Mechanism rot/s and rot/s^2; fallback values from turret.json maxVelocity/maxAcceleration.
+      double maxVelMechRotPerSec = trapConstraints.map(c -> c.maxVelocity).orElse(1080.0 / 360.0);
+      double maxAccelMechRotPerSecSq =
+          trapConstraints.map(c -> c.maxAcceleration).orElse(5080.0 / 360.0);
+      double lowerLimitRot =
+          turretConfig.getMechanismLowerLimit().orElse(Degrees.of(-150)).in(Rotations);
+      double upperLimitRot =
+          turretConfig.getMechanismUpperLimit().orElse(Degrees.of(150)).in(Rotations);
+
+      Object rawController = turret.getMotorController().getMotorController();
+      if (rawController instanceof com.ctre.phoenix6.hardware.TalonFX talonFXRaw) {
+        // Load feedforward from YAMS mechanism config (populated from turret.json motorSystemId).
+        // Turret is a Pivot so getArmFeedforward() contains the characterised kS/kV/kA in SI units.
+        // Fallback values match turret.json in case the YAMS FF was not set.
+        ArmFeedforward yamsFf = turretConfig.getArmFeedforward().orElse(null);
+        double kS = yamsFf != null ? yamsFf.getKs() : 30;
+        double kV = yamsFf != null ? yamsFf.getKv() : 0.0;
+        double kA = yamsFf != null ? yamsFf.getKa() : 2.0;
+        SmartTurretConfig smartConfig =
+            new SmartTurretConfig.Builder()
+                .withTalonFX(talonFXRaw)
+                .withYAMSController(turret.getMotorController())
+                .withGearRatio(30.0)
+                .withMotionConstraints(maxVelMechRotPerSec, maxAccelMechRotPerSecSq)
+                .withSeekingPID(255, 0, 50) // Initial values from turret.json
+                .withTrackingPID(550, 0, 200) // Start same, tune separately
+                .withFeedforward(kS, kV, kA)
+                .withSeekingThreshold(Degrees.of(8).in(Rotations))
+                .withHysteresisBuffer(Degrees.of(12).in(Rotations))
+                .withSoftLimits(lowerLimitRot, upperLimitRot)
+                .build();
+
+        smartTurretController = new SmartTurretController(smartConfig);
+        // Reset controller to the CRT-solved initial position.
+        smartTurretController.reset(calculatedAngle.in(Rotations), 0);
+      }
+    }
+
+    turret.min().or(turret.max()).onTrue(Commands.runOnce(() -> turret.getMotor().setDutyCycle(0)));
+  }
+
+  public ShotCalculator.ShootingParameters getShootingParameters(
+      Supplier<Pose2d> robotPoseSupplier, Supplier<Translation2d> targetPositionSupplier) {
+    ShotCalculator.getInstance().clearShootingParameters();
+    return ShotCalculator.getInstance()
+        .getParameters(
+            robotToTurret,
+            Rotation2d.fromDegrees(turret.getAngle().in(Degrees)),
+            robotPoseSupplier,
+            targetPositionSupplier);
+  }
+
+  @Override()
+  /** Updating launcher sensor data, calculates shot parameters, and populates input telemetry */
+  public void updateInputs(LauncherIOInputs inputs) {
+    SmartDashboard.putNumber("EasyCRT/Encoder 40", crtSensor40.getAsDouble("angle"));
+    SmartDashboard.putNumber("EasyCRT/Enc 2", easyCrt.getAbsoluteEncoder2Angle().in(Degrees));
+    SmartDashboard.putNumber("EasyCRT/Encoder 36", crtSensor36.getAsDouble("angle"));
+    SmartDashboard.putNumber("EasyCRT/Enc 1", easyCrt.getAbsoluteEncoder1Angle().in(Degrees));
+    SmartDashboard.putNumber(
+        "Distance to tag 27",
+        drivetrain
+            .getPoseEstimator()
+            .getCurrentPose3d()
+            .toPose2d()
+            .minus(AprilTags.aprilTagFieldLayout.getTagPose(21).get().toPose2d())
+            .getTranslation()
+            .getNorm());
+    // Angle calculatedAngle =
+    // easyCrtSolver.getAngleOptional().orElse(Degrees.of(0.0));
+    // SmartDashboard.putNumber("CRT Angle", calculatedAngle.in(Degrees));
+    // SmartDashboard.putString("CRT Status", easyCrtSolver.getLastStatus().name());
+    // SmartDashboard.putNumber("CRT Error Rot",
+    // easyCrtSolver.getLastErrorRotations());
+
+    Optional<Translation2d> targetPose = determineTarget();
+    inputs.isValidCalculation = false;
+    SmartDashboard.putNumber("Flywheel Multiplier", ShotCalculator.getFlywheelMultiplier());
+    if (targetPose.isPresent()) {
+      ShotCalculator.getInstance().clearShootingParameters();
+      ShotCalculator.ShootingParameters params =
+          ShotCalculator.getInstance()
+              .getParameters(
+                  robotToTurret,
+                  Rotation2d.fromDegrees(turret.getAngle().in(Degrees)),
+                  () -> Rebuilt.drivetrain.getPoseEstimator().getCurrentPose(),
+                  () -> targetPose.get());
+      if (params != null) {
+        inputs.isValidCalculation = params.isValid();
+        inputs.hoodAngleCalculated = Radian.of(params.hoodAngle());
+        inputs.turretAngleCalculated = params.turretAngle().getMeasure();
+        inputs.flyWheelSpeedCalculated =
+            RPM.of(params.flywheelSpeed() * ShotCalculator.getFlywheelMultiplier());
+        inputs.distanceToVirtualTarget = params.distanceToVirtualTarget();
+        inputs.turretFeedforwardRadPerSec = params.solution().turretFeedforwardRadPerSec();
+        inputs.turretFeedforwardAccelRadPerSecSq =
+            (inputs.turretFeedforwardRadPerSec - previousTurretVelocityRadPerSec)
+                / org.frc5010.common.constants.Constants.loopPeriodSecs;
+        previousTurretVelocityRadPerSec = inputs.turretFeedforwardRadPerSec;
+      }
+      inputs.robotToTarget = LauncherCommands.getRobotToTarget(targetPose.get());
+
+      inputs.targetDistance = Meters.of(inputs.robotToTarget.getDistance(new Translation2d()));
+    }
+    /** Reads the desired flywheel, hood, and turret setpoints */
+    inputs.flyWheelSpeedDesired =
+        flyWheel
+            .getMotorController()
+            .getMechanismSetpointVelocity()
+            .map(it -> it)
+            .orElse(RPM.of(0.0));
+    inputs.hoodAngleDesired =
+        hood.getMotorController().getMechanismPositionSetpoint().orElse(Degrees.of(0.0));
+    // Read turret desired angle from the SmartTurretController's goal, not YAMS (which is
+    // bypassed).
+    inputs.turretAngleDesired =
+        smartTurretController != null
+            ? Rotations.of(smartTurretController.getGoalPositionMechRot())
+            : turret.getMotorController().getMechanismPositionSetpoint().orElse(Degrees.of(0.0));
+
+    inputs.flyWheelSpeedActual = flyWheel.getSpeed();
+    inputs.hoodAngleActual = hood.getAngle();
+    inputs.turretAngleActual = turret.getAngle();
+
+    inputs.flyWheelSpeedError = inputs.flyWheelSpeedActual.minus(inputs.flyWheelSpeedDesired);
+    inputs.hoodAngleError = inputs.hoodAngleActual.minus(inputs.hoodAngleDesired).in(Degrees);
+    inputs.turretAngleError = inputs.turretAngleActual.minus(inputs.turretAngleDesired).in(Degrees);
+
+    inputs.flyWheelSpeedAtGoal =
+        Math.abs(inputs.flyWheelSpeedError.in(RPM)) <= Constants.Launcher.SHOOTER_TOLERANCE_RPM;
+    inputs.hoodAngleAtGoal =
+        Math.abs(inputs.hoodAngleError) <= Constants.Launcher.HOOD_ANGLE_TOLERANCE_DEGREES;
+    inputs.turretAngleAtGoal =
+        Math.abs(inputs.turretAngleError) <= Constants.Launcher.TURRET_ANGLE_TOLERANCE_DEGREES;
+
+    inputs.hoodVelocity = hood.getMotorController().getMechanismVelocity().in(Degrees.per(Second));
+    inputs.turretVelocity =
+        turret.getMotorController().getMechanismVelocity().in(Degrees.per(Second));
+    inputs.flyWheelMotorOutput = flyWheel.getMotor().getStatorCurrent().in(Amps);
+    isNearTrench();
+  }
+
+  /** Configuring the shot calculator with limits and constraints */
+  @Override
+  public void configureShotCalculator(ShotCalculator shotCalculator) {
+    var turretConfig = turret.getMotorController().getConfig();
+
+    shotCalculator.setShotTables(ShotCalculator.createDefaultTables());
+
+    // Turret angular limits and aim tolerance — read directly from the YAMS config so they stay
+    // in sync with the soft-limit values defined in launcher/turret.json.
+    Rotation2d aimTolerance =
+        Rotation2d.fromDegrees(
+            turretConfig.getClosedLoopTolerance().orElse(Degrees.of(10.0)).in(Degrees));
+    shotCalculator.setTurretConstraints(
+        Rotation2d.fromDegrees(turretConfig.getMechanismLowerLimit().get().in(Degrees)),
+        Rotation2d.fromDegrees(turretConfig.getMechanismUpperLimit().get().in(Degrees)),
+        aimTolerance);
+
+    // Trapezoidal motion profile constraints — read from the YAMS config (populated from
+    // launcher/turret.json motorSystemId.maxVelocity / maxAcceleration).
+    // YAMS stores these in rot/s and rot/s², so multiply by 2π to get rad/s and rad/s².
+    var trapConstraints = turretConfig.getTrapezoidProfile();
+    double maxVelRadPerSec =
+        trapConstraints.map(c -> c.maxVelocity * 2.0 * Math.PI).orElse(Math.toRadians(1080.0));
+    double maxAccelRadPerSecSq =
+        trapConstraints.map(c -> c.maxAcceleration * 2.0 * Math.PI).orElse(Math.toRadians(360.0));
+    shotCalculator.setTurretMotionConstraints(maxVelRadPerSec, maxAccelRadPerSecSq, 0.85);
+
+    // Provide the turret motor's actual velocity to the shot calculator for
+    // velocity-aware settling time estimation. Uses encoder velocity (not profile velocity)
+    // to avoid a feedback loop between the solver and the controller.
+    if (smartTurretController != null) {
+      shotCalculator.setTurretVelocitySupplier(smartTurretController::getActualVelocityRadPerSec);
+    }
+  }
+
+  @Override
+  public SmartTurretController getSmartTurretController() {
+    return smartTurretController;
+  }
+
+  /** Sets the flywheel motor's duty cycle */
+  public void runShooter(double speed) {
+    flyWheel.getMotor().setDutyCycle(speed);
+  }
+
+  /** Sets the flywheel motor's angular velocity */
+  public void setFlyWheelVelocity(AngularVelocity speed) {
+    flyWheel.getMotor().setVelocity(speed);
+  }
+
+  /** Sets the hood angle and overrides the requested angle if the hood is near the trench */
+  public void setHoodAngle(Angle angle) {
+    hood.getMotorController().setPosition(angle);
+  }
+
+  /** Sets the low hard limit to 30 degrees and updates LED's */
+  public void setHoodAngleLow() {
+    hood.getMotorController()
+        .setPosition(hood.getArmConfig().getLowerHardLimit().orElse(Degrees.of(30)));
+    LEDStrip.changeSegmentPattern(ConfigConstants.ALL_LEDS, LEDStrip.getSolidPattern(Color.kGreen));
+  }
+
+  /** Sets the angle of the turret via the SmartTurretController (zero feedforward). */
+  public void setTurretRotation(Angle angle) {
+    if (angle.gt(turretHighLimit)) {
+      SmartDashboard.putBoolean("Launcher/Turret Limit", true);
+      angle = turretHighLimit;
+    } else if (angle.lt(turretLowLimit)) {
+      SmartDashboard.putBoolean("Launcher/Turret Limit", true);
+      angle = turretLowLimit;
+    } else {
+      SmartDashboard.putBoolean("Launcher/Turret Limit", false);
+    }
+    if (smartTurretController != null) {
+      smartTurretController.setTarget(angle, 0.0, 0.0);
+    } else {
+      turret.getMotorController().setPosition(angle);
+    }
+  }
+
+  /**
+   * Sets the turret angle with velocity and acceleration feedforward for the SmartTurretController.
+   *
+   * @param angle desired turret mechanism angle
+   * @param feedforwardRadPerSec angular velocity feedforward in rad/s (mechanism units)
+   * @param accelerationRadPerSecSq angular acceleration feedforward in rad/s^2 (mechanism units)
+   */
+  @Override
+  public void setTurretRotationWithFeedforward(
+      Angle angle, double feedforwardRadPerSec, double accelerationRadPerSecSq) {
+    if (angle.gt(turretHighLimit)) {
+      SmartDashboard.putBoolean("Launcher/Turret Limit", true);
+      angle = turretHighLimit;
+    } else if (angle.lt(turretLowLimit)) {
+      SmartDashboard.putBoolean("Launcher/Turret Limit", true);
+      angle = turretLowLimit;
+    } else {
+      SmartDashboard.putBoolean("Launcher/Turret Limit", false);
+    }
+
+    if (smartTurretController != null) {
+      smartTurretController.setTarget(angle, feedforwardRadPerSec, accelerationRadPerSecSq);
+    } else {
+      // Fallback: YAMS setPosition without feedforward
+      turret.getMotorController().setPosition(angle);
+    }
+  }
+
+  /** Converts the flywheel angular velocity into speed */
+  public LinearVelocity getFlyWheelExitSpeed(AngularVelocity velocity) {
+    return MetersPerSecond.of(
+        flyWheel.getShooterConfig().getCircumference().in(Meters)
+            * Math.PI // This is a total fudge on the math, but it gives us a more realistic exit
+            // velocity for the flywheel speeds we are commanding
+            * (velocity.in(RadiansPerSecond)));
+  }
+
+  /** Returns SysId command for the hood */
+  public Command getHoodSysIdCommand() {
+    return hood.sysId(Volts.of(4), Volts.of(0.5).per(Seconds), Seconds.of(8));
+  }
+
+  /** Runs sysid for the cahracterized hood motor and stops at limits */
+  public Command getHoodSysIdCommand(GenericSubsystem launcher) {
+    return SystemIdentification.getSysIdFullCommand(
+        SystemIdentification.angleSysIdRoutine(hood.getMotorController(), hood.getName(), launcher),
+        5,
+        3,
+        3,
+        () ->
+            hood.isNear(
+                    hood.getMotorController().getConfig().getMechanismUpperLimit().get(),
+                    Degrees.of(10))
+                .getAsBoolean(),
+        () ->
+            hood.isNear(
+                    hood.getMotorController().getConfig().getMechanismLowerLimit().get(),
+                    Degrees.of(10))
+                .getAsBoolean(),
+        () -> hood.getMotor().setDutyCycle(0));
+  }
+
+  public Command getTurretSysIdCommand() {
+    return turret.sysId(Volts.of(4), Volts.of(0.5).per(Seconds), Seconds.of(8));
+  }
+
+  /** Characterizes the turret */
+  public Command getTurretSysIdCommand(GenericSubsystem launcher) {
+    return SystemIdentification.getSysIdFullCommand(
+        SystemIdentification.angleSysIdRoutine(
+            turret.getMotorController(), turret.getName(), launcher),
+        5,
+        3.5,
+        3,
+        () ->
+            turret
+                .isNear(
+                    turret.getMotorController().getConfig().getMechanismUpperLimit().get(),
+                    Degrees.of(10))
+                .getAsBoolean(),
+        () ->
+            turret
+                .isNear(
+                    turret.getMotorController().getConfig().getMechanismLowerLimit().get(),
+                    Degrees.of(10))
+                .getAsBoolean(),
+        () -> turret.getMotor().setDutyCycle(0));
+  }
+
+  /** Applies voltage and measures hood velocity to characterize the feed forward */
+  public Command getHoodCharacterizationCommand(GenericSubsystem launcher) {
+    return SystemIdentification.feedforwardCharacterization(
+        launcher,
+        (Voltage voltage) -> hood.getMotor().setVoltage(voltage),
+        () -> hood.getMotorController().getMechanismVelocity().in(Degrees.per(Second)));
+  }
+
+  /** Applies voltage and measures turret velocity to characterize the feedfoward */
+  public Command getTurretCharacterizationCommand(GenericSubsystem launcher) {
+    return SystemIdentification.feedforwardCharacterization(
+        launcher,
+        (Voltage voltage) -> turret.getMotor().setVoltage(voltage),
+        () -> turret.getMotorController().getMechanismVelocity().in(Degrees.per(Second)));
+  }
+
+  /** sets the flywheel, hood, and turret motor duty cycles to 0, which stops the motors */
+  public void stopAllMotors() {
+    flyWheel.getMotor().setDutyCycle(0);
+    hood.getMotor().setDutyCycle(0);
+    turret.getMotor().setDutyCycle(0);
+  }
+
+  public Command getFlyWheelSysIdCommand() {
+    return flyWheel.sysId(Volts.of(8), Volts.of(0.5).per(Seconds), Seconds.of(8));
+  }
+
+  public boolean isNearTrench() {
+    Pose2d current = drivetrain.getPoseEstimator().getCurrentPose();
+    double currentX = current.getX();
+    double currentY = current.getY();
+
+    return FieldRegions.isNearTrench(currentX, currentY);
+  }
+
+  public Optional<Translation2d> determineTarget() {
+    Pose2d current = drivetrain.getPoseEstimator().getCurrentPose();
+    return FieldRegions.determineTargetPose(current);
+  }
+
+  public Command getFlyWheelSysIdCommand(GenericSubsystem launcher) {
+    return SystemIdentification.getSysIdFullCommand(
+        SystemIdentification.rpmSysIdRoutine(
+            flyWheel.getMotorController(), flyWheel.getName(), launcher),
+        8,
+        3,
+        3);
+  }
+
+  @Override
+  public Command getTurretQuasistaticCommand(GenericSubsystem launcher) {
+    if (smartTurretController == null) return Commands.none();
+    return new frc.robot.rebuilt.commands.TurretQuasistaticCommand(smartTurretController, launcher);
+  }
+
+  @Override
+  public Command getTurretDynamicCommand(GenericSubsystem launcher) {
+    if (smartTurretController == null) return Commands.none();
+    return new frc.robot.rebuilt.commands.TurretDynamicCommand(smartTurretController, launcher);
+  }
+
+  @Override
+  public Command getTurretKsMapCommand(GenericSubsystem launcher) {
+    if (smartTurretController == null) return Commands.none();
+    return new frc.robot.rebuilt.commands.TurretKsMapCommand(
+        smartTurretController, turretLowLimit, turretHighLimit, launcher);
+  }
+
+  @Override
+  public Command getTurretTrackingTuneCommand(GenericSubsystem launcher) {
+    if (smartTurretController == null) return Commands.none();
+    return new frc.robot.rebuilt.commands.TurretTrackingTuneCommand(
+        smartTurretController, launcher);
+  }
+
+  @Override
+  public Command getTurretSeekingTuneCommand(GenericSubsystem launcher) {
+    if (smartTurretController == null) return Commands.none();
+    return new frc.robot.rebuilt.commands.TurretSeekingTuneCommand(smartTurretController, launcher);
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOSim.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOSim.java
new file mode 100644
index 00000000..d1de3ce0
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOSim.java
@@ -0,0 +1,117 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.Launcher;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.rebuilt.FieldConstants;
+import frc.robot.rebuilt.Rebuilt;
+import frc.robot.rebuilt.commands.IndexerCommands.IndexerState;
+import frc.robot.rebuilt.subsystems.Indexer.Indexer;
+import frc.robot.rebuilt.subsystems.intake.IntakeIOSim;
+import java.util.Map;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.littletonrobotics.junction.Logger;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceProjectile;
+import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly;
+
+/** Add your docs here. */
+public class LauncherIOSim extends LauncherIOReal {
+  protected GamePieceProjectile gamePieceProjectile;
+  protected Map<String, Object> devices;
+
+  public LauncherIOSim(Map<String, Object> devices, Map<String, GenericSubsystem> subsystems) {
+    super(devices, subsystems);
+    IntakeIOSim.intakeSimulation.addGamePiecesToIntake(8);
+    // Start with 8 gamepieces in the
+    // intake
+  }
+
+  @Override
+  /** Configures the shot calculator and calculates measurements for parts of the lancher */
+  public void configureShotCalculator(ShotCalculator shotCalculator) {
+    super.configureShotCalculator(shotCalculator);
+    double circumferenceMeters = flyWheel.getShooterConfig().getCircumference().in(Meters);
+    double wheelRadiusMeters = circumferenceMeters / (2.0 * Math.PI);
+    double minFlywheelRadPerSec =
+        flyWheel.getShooterConfig().getLowerSoftLimit().orElse(RPM.of(0.0)).in(RadiansPerSecond);
+    double maxFlywheelRadPerSec =
+        flyWheel.getShooterConfig().getUpperSoftLimit().orElse(RPM.of(5000.0)).in(RadiansPerSecond);
+    /** Reads the hood angle limits */
+    Rotation2d minHoodAngle =
+        Rotation2d.fromDegrees(
+            hood.getMotorController().getConfig().getMechanismLowerLimit().get().in(Degrees));
+    Rotation2d maxHoodAngle =
+        Rotation2d.fromDegrees(
+            hood.getMotorController().getConfig().getMechanismUpperLimit().get().in(Degrees));
+    Rotation2d hoodStep = Rotation2d.fromDegrees(0.5);
+
+    double launchHeight = flyWheel.getRelativeMechanismPosition().getZ();
+    double targetHeight = FieldConstants.Hub.height;
+    /** Creates ballistic configuration for the shot calculator */
+    ShotCalculator.BallisticConfig config =
+        new ShotCalculator.BallisticConfig(
+            1.0,
+            6.0,
+            0.1,
+            minHoodAngle,
+            maxHoodAngle,
+            hoodStep,
+            minFlywheelRadPerSec,
+            maxFlywheelRadPerSec,
+            wheelRadiusMeters,
+            launchHeight,
+            targetHeight,
+            0.0,
+            9.80665,
+            Math.toRadians(90.0));
+
+    shotCalculator.setBallisticConfig(config);
+    ShotCalculator.ShotTables simTables = ShotCalculator.createBallisticTables(config);
+    shotCalculator.setShotTables(simTables);
+  }
+
+  @Override
+  public void updateSimulation(Launcher launcher, Indexer indexer) {
+    int amount = IntakeIOSim.intakeSimulation.getGamePiecesAmount();
+    // Update simulated mechanism states here
+    // We should simulate a shot rate of about 10-15 gamepieces per second
+    // Every other time this is called, determine a randome number and if > 0.5, shoot a gamepiece.
+    // This would mean we try to shoot 25 times per second, and on average shoot about 12-13
+    // gamepieces per second.
+    if (Math.random() > 0.5 && amount > 0) {
+      if ((indexer.isCurrent(IndexerState.FEED) && launcher.isShooting())
+          || (indexer.isCurrent(IndexerState.FORCE))) {
+        if (IntakeIOSim.intakeSimulation.obtainGamePieceFromIntake()) {
+          Pose2d worldPose = Rebuilt.drivetrain.getPoseEstimator().getCurrentPose();
+          gamePieceProjectile =
+              new RebuiltFuelOnFly(
+                      worldPose.getTranslation(),
+                      flyWheel.getRelativeMechanismPosition().toTranslation2d(),
+                      Rebuilt.drivetrain.getFieldVelocity(),
+                      Rotation2d.fromDegrees(
+                          worldPose.getRotation().getMeasure().plus(turret.getAngle()).in(Degrees)),
+                      flyWheel.getRelativeMechanismPosition().getMeasureZ(),
+                      getFlyWheelExitSpeed(flyWheel.getSpeed()),
+                      Degrees.of(90.0).minus(hood.getAngle()))
+                  .withProjectileTrajectoryDisplayCallBack(
+                      (pose3ds) -> {
+                        Logger.recordOutput(
+                            "Launcher/GamePieceTrajectory", pose3ds.toArray(Pose3d[]::new));
+                      });
+          SimulatedArena.getInstance().addGamePieceProjectile(gamePieceProjectile);
+          // Create a new gamepiece on-the-fly and add it to the field simulation
+        }
+      }
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/ShotCalculator.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/ShotCalculator.java
new file mode 100644
index 00000000..9df35d19
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/ShotCalculator.java
@@ -0,0 +1,630 @@
+// Copyright (c) 2025-2026 Littleton Robotics
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.Launcher;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.interpolation.InterpolatingTreeMap;
+import edu.wpi.first.math.interpolation.InverseInterpolator;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.Distance;
+import frc.robot.rebuilt.Rebuilt;
+import frc.robot.rebuilt.subsystems.Launcher.TurretControlPhysics.AimingSolution;
+import java.util.Map;
+import java.util.TreeMap;
+import java.util.function.BiFunction;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+import lombok.experimental.ExtensionMethod;
+import org.frc5010.common.constants.Constants;
+import org.frc5010.common.utils.geometry.AllianceFlipUtil;
+import org.frc5010.common.utils.geometry.GeomUtil;
+import org.littletonrobotics.junction.Logger;
+
+@ExtensionMethod({GeomUtil.class})
+/** Calculates the turret and hood angle, and the flywheel speed for shooting */
+public class ShotCalculator {
+  private static ShotCalculator instance;
+
+  private final LinearFilter turretAngleFilter =
+      LinearFilter.movingAverage((int) (0.1 / Constants.loopPeriodSecs));
+  private final LinearFilter hoodAngleFilter =
+      LinearFilter.movingAverage((int) (0.1 / Constants.loopPeriodSecs));
+
+  private Rotation2d lastTurretAngle;
+  private double lastHoodAngle;
+  private Rotation2d turretAngle;
+  private double hoodAngle = Double.NaN;
+  private double turretVelocity;
+  private double hoodVelocity;
+  private TurretControlPhysics turretControlPhysics;
+  private Translation2d cachedTurretOffset;
+  private Rotation2d minTurretAngle = Rotation2d.fromDegrees(-165.0);
+  private Rotation2d maxTurretAngle = Rotation2d.fromDegrees(165.0);
+  private Rotation2d feedforwardPaddingAngle = Rotation2d.fromDegrees(10.0);
+  private double settlingGain = 0.00;
+  // Default turret motion constraints (overridden via setTurretMotionConstraints).
+  // These represent the practical maximum velocity (360 °/s) and acceleration (720 °/s²)
+  // until real SysId values are provided.
+  private double turretMaxVelocityRadPerSec = Math.toRadians(360.0);
+  private double turretMaxAccelRadPerSecSq = Math.toRadians(720.0);
+  private BiFunction<Double, Double, Double> settlingTimeFunction =
+      TurretControlPhysics.velocityAwareSettlingTimeFunction(
+          turretMaxVelocityRadPerSec, turretMaxAccelRadPerSecSq);
+  private DoubleSupplier turretVelocitySupplier = () -> 0.0;
+  private final String targetName = "Target";
+  private final String lookAhead = "Lookahead";
+  private final String virtualTarget = "VirtualTarget";
+  private final String turret = "Turret";
+
+  public static ShotCalculator getInstance() {
+    if (instance == null) instance = new ShotCalculator();
+    return instance;
+  }
+  /** Stores calculated shooting parameters */
+  public record ShootingParameters(
+      boolean isValid,
+      Rotation2d turretAngle,
+      double turretVelocity,
+      double hoodAngle,
+      double hoodVelocity,
+      double flywheelSpeed,
+      Distance distanceToVirtualTarget,
+      AimingSolution solution) {}
+
+  // Cache parameters
+  private ShootingParameters latestParameters = null;
+  public static double flywheelMultiplier = 1.05;
+
+  private static double minDistance;
+  private static double maxDistance;
+  private static double phaseDelay;
+  private static final InterpolatingTreeMap<Double, Rotation2d> shotHoodAngleMap =
+      new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate);
+  private static final InterpolatingDoubleTreeMap shotFlywheelSpeedMap =
+      new InterpolatingDoubleTreeMap();
+  private static final InterpolatingDoubleTreeMap timeOfFlightMap =
+      new InterpolatingDoubleTreeMap();
+
+  public record ShotTables(
+      Map<Double, Rotation2d> hoodAngles,
+      Map<Double, Double> flywheelSpeeds,
+      Map<Double, Double> timeOfFlightSeconds,
+      double minDistanceMeters,
+      double maxDistanceMeters,
+      double phaseDelaySeconds) {
+    /** returns a copy of ShotTables with updated phase delay */
+    public ShotTables withPhaseDelaySeconds(double newPhaseDelaySeconds) {
+      return new ShotTables(
+          hoodAngles,
+          flywheelSpeeds,
+          timeOfFlightSeconds,
+          minDistanceMeters,
+          maxDistanceMeters,
+          newPhaseDelaySeconds);
+    }
+  }
+
+  public static void incrementFlywheelMultiplier(double amount) {
+    ShotCalculator.flywheelMultiplier += amount;
+  }
+
+  public static double getFlywheelMultiplier() {
+    return ShotCalculator.flywheelMultiplier;
+  }
+  /** Stores configuration values for generating ballistic shot tables */
+  public record BallisticConfig(
+      double minDistanceMeters,
+      double maxDistanceMeters,
+      double distanceStepMeters,
+      Rotation2d minHoodAngle,
+      Rotation2d maxHoodAngle,
+      Rotation2d hoodAngleStep,
+      double minFlywheelRadPerSec,
+      double maxFlywheelRadPerSec,
+      double wheelRadiusMeters,
+      double launchHeightMeters,
+      double targetHeightMeters,
+      double phaseDelaySeconds,
+      double gravityMetersPerSecondSquared,
+      double hoodAngleReferenceRadians) {}
+
+  static {
+    applyShotTables(createDefaultTables());
+  }
+  /** Creates default hood angle, flywheel speeds, and time of light tables */
+  public static ShotTables createDefaultTables() {
+    final double offset = 0.5969;
+    return new ShotTables(
+        Map.ofEntries(
+            Map.entry(2.0796297600462808, Rotation2d.fromDegrees(33.0)),
+            Map.entry(2.3644814757115706, Rotation2d.fromDegrees(34.0)),
+            Map.entry(2.7648598116063776, Rotation2d.fromDegrees(35.0)),
+            Map.entry(3.2194779503261004, Rotation2d.fromDegrees(38.0)),
+            Map.entry(3.53088512698516, Rotation2d.fromDegrees(39.0)),
+            Map.entry(3.9268571046742813, Rotation2d.fromDegrees(40.0)),
+            Map.entry(4.317290273504823, Rotation2d.fromDegrees(42.0)),
+            Map.entry(4.540307519714445, Rotation2d.fromDegrees(43.0)),
+            Map.entry(5.77893560525366, Rotation2d.fromDegrees(45.0)),
+            Map.entry(6.35214199070158, Rotation2d.fromDegrees(47.0)),
+            Map.entry(10.990685758149123, Rotation2d.fromDegrees(50.0)),
+            Map.entry(13.024035615135324, Rotation2d.fromDegrees(55.0))),
+        Map.ofEntries(
+            Map.entry(2.0796297600462808, 97.0),
+            Map.entry(2.3644814757115706, 103.0),
+            Map.entry(2.7648598116063776, 105.0),
+            Map.entry(3.2194779503261004, 107.0),
+            Map.entry(3.53088512698516, 110.0),
+            Map.entry(3.9268571046742813, 113.0),
+            Map.entry(4.317290273504823, 115.0),
+            Map.entry(4.540307519714445, 117.0),
+            Map.entry(5.77893560525366, 127.0),
+            Map.entry(6.35214199070158, 133.0),
+            Map.entry(10.990685758149123, 155.0),
+            Map.entry(13.024035615135324, 170.0)),
+        Map.ofEntries(
+            Map.entry(2.11, 1.04),
+            Map.entry(3.92, 1.19),
+            Map.entry(4.10, 1.22),
+            Map.entry(5.58, 1.28)),
+        0.7,
+        100.0,
+        0.03);
+  }
+
+  public static ShotTables createBallisticTables(BallisticConfig config) {
+    if (config == null || config.wheelRadiusMeters() <= 0.0) {
+      return createDefaultTables();
+    }
+
+    Map<Double, Rotation2d> hoodAngles = new TreeMap<>();
+    Map<Double, Double> flywheelSpeeds = new TreeMap<>();
+    Map<Double, Double> timeOfFlight = new TreeMap<>();
+
+    double minValid = Double.POSITIVE_INFINITY;
+    double maxValid = 0.0;
+    double minDistance = config.minDistanceMeters();
+    double maxDistance = config.maxDistanceMeters();
+    double distanceStep = Math.max(config.distanceStepMeters(), 0.05);
+    double angleStep = Math.max(config.hoodAngleStep().getRadians(), Math.toRadians(0.25));
+    double hoodReference = config.hoodAngleReferenceRadians();
+    double minAngle = hoodReference - config.maxHoodAngle().getRadians();
+    double maxAngle = hoodReference - config.minHoodAngle().getRadians();
+    double gravity = config.gravityMetersPerSecondSquared();
+    double heightDelta = config.targetHeightMeters() - config.launchHeightMeters();
+
+    for (double distance = minDistance; distance <= maxDistance + 1e-6; distance += distanceStep) {
+      BallisticSolution solution =
+          solveBallistic(
+              distance,
+              heightDelta,
+              gravity,
+              minAngle,
+              maxAngle,
+              angleStep,
+              config.wheelRadiusMeters(),
+              config.minFlywheelRadPerSec(),
+              config.maxFlywheelRadPerSec());
+      if (solution == null) {
+        continue;
+      }
+      hoodAngles.put(distance, Rotation2d.fromRadians(hoodReference - solution.angleRadians()));
+      flywheelSpeeds.put(distance, solution.flywheelRadPerSec());
+      timeOfFlight.put(distance, solution.timeOfFlightSeconds());
+      minValid = Math.min(minValid, distance);
+      maxValid = Math.max(maxValid, distance);
+    }
+
+    if (hoodAngles.isEmpty()) {
+      return createDefaultTables();
+    }
+
+    double minRange = Double.isFinite(minValid) ? minValid : minDistance;
+    double maxRange = maxValid > 0.0 ? maxValid : maxDistance;
+
+    return new ShotTables(
+        hoodAngles, flywheelSpeeds, timeOfFlight, minRange, maxRange, config.phaseDelaySeconds());
+  }
+
+  private record BallisticSolution(
+      double angleRadians, double flywheelRadPerSec, double timeOfFlightSeconds) {}
+
+  private static BallisticSolution solveBallistic(
+      double distanceMeters,
+      double heightDeltaMeters,
+      double gravityMetersPerSecondSquared,
+      double minAngleRadians,
+      double maxAngleRadians,
+      double angleStepRadians,
+      double wheelRadiusMeters,
+      double minFlywheelRadPerSec,
+      double maxFlywheelRadPerSec) {
+    double bestFlywheel = Double.POSITIVE_INFINITY;
+    double bestAngle = 0.0;
+    double bestTime = 0.0;
+
+    for (double angle = minAngleRadians;
+        angle <= maxAngleRadians + 1e-6;
+        angle += angleStepRadians) {
+      double cos = Math.cos(angle);
+      double tan = Math.tan(angle);
+      double denominator = distanceMeters * tan - heightDeltaMeters;
+      if (denominator <= 0.0 || Math.abs(cos) < 1e-6) {
+        continue;
+      }
+
+      double velocitySquared =
+          gravityMetersPerSecondSquared
+              * distanceMeters
+              * distanceMeters
+              / (2.0 * cos * cos * denominator);
+      if (velocitySquared <= 0.0) {
+        continue;
+      }
+
+      double velocity = Math.sqrt(velocitySquared);
+      double flywheelRadPerSec = velocity / wheelRadiusMeters;
+      if (flywheelRadPerSec < minFlywheelRadPerSec || flywheelRadPerSec > maxFlywheelRadPerSec) {
+        continue;
+      }
+
+      if (flywheelRadPerSec < bestFlywheel) {
+        bestFlywheel = flywheelRadPerSec;
+        bestAngle = angle;
+        bestTime = distanceMeters / (velocity * cos);
+      }
+    }
+
+    if (!Double.isFinite(bestFlywheel)) {
+      return null;
+    }
+
+    return new BallisticSolution(bestAngle, bestFlywheel, bestTime);
+  }
+
+  /**
+   * Get the interpolated hood angle (degrees) from the current lookup table for a given distance.
+   *
+   * @param distanceMeters distance to target in meters
+   * @return hood angle in degrees from the lookup table, or NaN if unavailable
+   */
+  public double getLookupHoodAngleDegrees(double distanceMeters) {
+    Rotation2d value = shotHoodAngleMap.get(distanceMeters);
+    return value != null ? value.getDegrees() : Double.NaN;
+  }
+
+  public boolean hasValidShot() {
+    if (latestParameters != null) {
+      return latestParameters.solution.isPossible();
+    }
+    return false;
+  }
+
+  /**
+   * Get the interpolated flywheel speed from the current lookup table for a given distance.
+   *
+   * @param distanceMeters distance to target in meters
+   * @return flywheel speed from the lookup table, or NaN if unavailable
+   */
+  public double getLookupFlywheelSpeed(double distanceMeters) {
+    Double value = shotFlywheelSpeedMap.get(distanceMeters);
+    return value != null ? value : Double.NaN;
+  }
+
+  /**
+   * Compute a ballistic guess for the given distance using the stored ballistic config. Returns
+   * null if no ballistic config has been provided.
+   *
+   * @param distanceMeters distance to target in meters
+   * @return a double[] of {hoodAngleDegrees, flywheelSpeed, timeOfFlightSeconds}, or null
+   */
+  public double[] getBallisticGuess(double distanceMeters) {
+    if (ballisticConfig == null) {
+      return null;
+    }
+    double hoodReference = ballisticConfig.hoodAngleReferenceRadians();
+    double minAngle = hoodReference - ballisticConfig.maxHoodAngle().getRadians();
+    double maxAngle = hoodReference - ballisticConfig.minHoodAngle().getRadians();
+    double angleStep = Math.max(ballisticConfig.hoodAngleStep().getRadians(), Math.toRadians(0.25));
+    double heightDelta =
+        ballisticConfig.targetHeightMeters() - ballisticConfig.launchHeightMeters();
+    BallisticSolution solution =
+        solveBallistic(
+            distanceMeters,
+            heightDelta,
+            ballisticConfig.gravityMetersPerSecondSquared(),
+            minAngle,
+            maxAngle,
+            angleStep,
+            ballisticConfig.wheelRadiusMeters(),
+            ballisticConfig.minFlywheelRadPerSec(),
+            ballisticConfig.maxFlywheelRadPerSec());
+    if (solution == null) {
+      return null;
+    }
+    double hoodAngleDegrees = Math.toDegrees(hoodReference - solution.angleRadians());
+    return new double[] {
+      hoodAngleDegrees, solution.flywheelRadPerSec(), solution.timeOfFlightSeconds()
+    };
+  }
+
+  /**
+   * Add or update a single data point in the live lookup tables.
+   *
+   * @param distanceMeters the distance key
+   * @param hoodAngleDegrees hood angle in degrees
+   * @param flywheelSpeed flywheel speed value
+   * @param timeOfFlightSeconds estimated time-of-flight in seconds
+   */
+  public void addDataPoint(
+      double distanceMeters,
+      double hoodAngleDegrees,
+      double flywheelSpeed,
+      double timeOfFlightSeconds) {
+    shotHoodAngleMap.put(distanceMeters, Rotation2d.fromDegrees(hoodAngleDegrees));
+    shotFlywheelSpeedMap.put(distanceMeters, flywheelSpeed);
+    timeOfFlightMap.put(distanceMeters, timeOfFlightSeconds);
+    if (distanceMeters < minDistance) {
+      minDistance = distanceMeters;
+    }
+    if (distanceMeters > maxDistance) {
+      maxDistance = distanceMeters;
+    }
+    latestParameters = null;
+    turretControlPhysics = null;
+  }
+
+  /** Store a ballistic config so that ballistic guesses can be computed on demand. */
+  private BallisticConfig ballisticConfig;
+
+  public void setBallisticConfig(BallisticConfig config) {
+    this.ballisticConfig = config;
+  }
+
+  public BallisticConfig getBallisticConfig() {
+    return ballisticConfig;
+  }
+
+  public void setShotTables(ShotTables tables) {
+    applyShotTables(tables);
+    latestParameters = null;
+    turretControlPhysics = null;
+  }
+
+  private static void applyShotTables(ShotTables tables) {
+    if (tables == null) {
+      return;
+    }
+    shotHoodAngleMap.clear();
+    tables.hoodAngles().forEach(shotHoodAngleMap::put);
+    shotFlywheelSpeedMap.clear();
+    tables.flywheelSpeeds().forEach(shotFlywheelSpeedMap::put);
+    timeOfFlightMap.clear();
+    tables.timeOfFlightSeconds().forEach(timeOfFlightMap::put);
+    minDistance = tables.minDistanceMeters();
+    maxDistance = tables.maxDistanceMeters();
+    phaseDelay = tables.phaseDelaySeconds();
+  }
+
+  public void setTurretConstraints(
+      Rotation2d minAngle, Rotation2d maxAngle, Rotation2d paddingAngle) {
+    if (minAngle != null) {
+      minTurretAngle = minAngle;
+    }
+    if (maxAngle != null) {
+      maxTurretAngle = maxAngle;
+    }
+    if (paddingAngle != null) {
+      feedforwardPaddingAngle = paddingAngle;
+    }
+    turretControlPhysics = null;
+  }
+
+  public void setSettlingTimeFunction(
+      BiFunction<Double, Double, Double> function, double newSettlingGain) {
+    if (function != null) {
+      settlingTimeFunction = function;
+    }
+    settlingGain = newSettlingGain;
+    turretControlPhysics = null;
+  }
+
+  /**
+   * Configures the turret settling-time function using the closed-form trapezoidal motion profile.
+   *
+   * <p>This replaces any previously supplied {@link #setSettlingTimeFunction} with the analytical
+   * result derived from the given peak velocity and acceleration. Call this once during robot init
+   * after measuring the turret's actual motion profile constraints via SysId.
+   *
+   * @param maxVelocityRadPerSec peak turret velocity in rad/s
+   * @param maxAccelRadPerSecSq peak turret acceleration in rad/s²
+   * @param newSettlingGain multiplier applied to the computed time (use ≤ 1.0 to avoid oscillation)
+   */
+  public void setTurretMotionConstraints(
+      double maxVelocityRadPerSec, double maxAccelRadPerSecSq, double newSettlingGain) {
+    turretMaxVelocityRadPerSec = maxVelocityRadPerSec;
+    turretMaxAccelRadPerSecSq = maxAccelRadPerSecSq;
+    settlingTimeFunction =
+        TurretControlPhysics.velocityAwareSettlingTimeFunction(
+            turretMaxVelocityRadPerSec, turretMaxAccelRadPerSecSq);
+    settlingGain = newSettlingGain;
+    turretControlPhysics = null;
+  }
+
+  /**
+   * Sets the supplier for the current turret angular velocity. This is used by the velocity-aware
+   * settling time function to account for turret momentum when predicting time-to-arrival.
+   *
+   * @param supplier a {@link DoubleSupplier} returning the current turret velocity in rad/s
+   */
+  public void setTurretVelocitySupplier(DoubleSupplier supplier) {
+    if (supplier != null) {
+      turretVelocitySupplier = supplier;
+    }
+  }
+
+  public ShootingParameters getParameters(
+      Translation2d turretRelativePosition,
+      Rotation2d turretRelativeAngle,
+      Supplier<Pose2d> robotPoseSupplier,
+      Supplier<Translation2d> targetPositionSupplier) {
+    if (latestParameters != null) {
+      return latestParameters;
+    }
+
+    // Snapshot current pose, field-relative velocity, and field-relative acceleration.
+    // Field velocity is used for linear extrapolation: x += vx*dt, y += vy*dt, heading += omega*dt.
+    // This matches the actual drivetrain behavior since the drive code already accounts for
+    // curvature (discretize) when commanding inputs to drive straight in field frame.
+    Pose2d estimatedPose = robotPoseSupplier.get();
+    ChassisSpeeds fieldVelocity = Rebuilt.drivetrain.getFieldVelocity();
+    ChassisSpeeds fieldAcceleration = Rebuilt.drivetrain.getFieldAcceleration();
+
+    // Apply phase delay using linear field-frame extrapolation.
+    Pose2d phaseDelayedPose = linearExtrapolatePose(estimatedPose, fieldVelocity, phaseDelay);
+
+    Translation2d target = AllianceFlipUtil.apply(targetPositionSupplier.get());
+    Pose2d turretPosition =
+        phaseDelayedPose.transformBy(
+            new Transform2d(
+                turretRelativePosition.getMeasureX(),
+                turretRelativePosition.getMeasureY(),
+                turretRelativeAngle));
+
+    TurretControlPhysics physics = getTurretControlPhysics(turretRelativePosition);
+    double currentTurretVelocityRadPerSec = turretVelocitySupplier.getAsDouble();
+    TurretControlPhysics.AimingSolution solution =
+        physics.solve(
+            target,
+            turretRelativeAngle,
+            currentTurretVelocityRadPerSec,
+            (timeSinceStartSeconds, lookaheadSeconds) -> {
+              // Linear field-frame pose extrapolation: no twist, just straight-line translation
+              // in the field frame plus proportional heading change.
+              Pose2d predictedPose =
+                  linearExtrapolatePose(phaseDelayedPose, fieldVelocity, lookaheadSeconds);
+              return new TurretControlPhysics.RobotState(
+                  predictedPose, fieldVelocity, fieldAcceleration);
+            });
+
+    double distanceToVirtualTarget = solution.effectiveDistanceMeters();
+    Rotation2d hoodSetpoint = shotHoodAngleMap.get(distanceToVirtualTarget);
+    Double flywheelSpeed = shotFlywheelSpeedMap.get(distanceToVirtualTarget);
+
+    turretAngle = solution.turretLocalHeading();
+    hoodAngle = hoodSetpoint != null ? hoodSetpoint.getRadians() : 0.0;
+    if (lastTurretAngle == null) lastTurretAngle = turretAngle;
+    if (Double.isNaN(lastHoodAngle)) lastHoodAngle = hoodAngle;
+    turretVelocity =
+        turretAngleFilter.calculate(
+            turretAngle.minus(lastTurretAngle).getRadians() / Constants.loopPeriodSecs);
+    hoodVelocity =
+        hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.loopPeriodSecs);
+    lastTurretAngle = turretAngle;
+    lastHoodAngle = hoodAngle;
+    latestParameters =
+        new ShootingParameters(
+            solution.isPossible(),
+            turretAngle,
+            turretVelocity,
+            hoodAngle,
+            hoodVelocity,
+            flywheelSpeed != null ? flywheelSpeed : 0.0,
+            Meters.of(distanceToVirtualTarget),
+            solution);
+
+    // Log calculated values
+    Logger.recordOutput("ShotCalculator/AimingStatus", solution.status().toString());
+    Logger.recordOutput(
+        "ShotCalculator/TurretToTargetDistance", solution.effectiveDistanceMeters());
+    Logger.recordOutput(
+        "ShotCalculator/VirtualTargetFieldPosition",
+        new Pose2d(solution.finalSolverState().virtualTargetFieldPos(), turretAngle));
+    Logger.recordOutput("ShotCalculator/FieldVelocity", fieldVelocity);
+    Logger.recordOutput("ShotCalculator/FieldAcceleration", fieldAcceleration);
+
+    Rebuilt.drivetrain
+        .getField2d()
+        .getObject(targetName)
+        .setPose(new Pose2d(target, target.getAngle()));
+    // Lookahead visualisation uses the same linear extrapolation
+    Pose2d lookaheadRobotPose =
+        linearExtrapolatePose(phaseDelayedPose, fieldVelocity, solution.estimatedTimeOfFlight());
+    Pose2d lookaheadTurretPose =
+        lookaheadRobotPose.transformBy(
+            new Transform2d(
+                turretRelativePosition.getMeasureX(),
+                turretRelativePosition.getMeasureY(),
+                turretRelativeAngle));
+    Rebuilt.drivetrain.getField2d().getObject(lookAhead).setPose(lookaheadTurretPose);
+    Pose2d virtualTargetPose = new Pose2d(solution.virtualTargetFieldPos(), turretAngle);
+    Rebuilt.drivetrain.getField2d().getObject(virtualTarget).setPose(virtualTargetPose);
+    Rebuilt.drivetrain.getField2d().getObject(turret).setPose(turretPosition);
+
+    return latestParameters;
+  }
+
+  /**
+   * Extrapolates a robot pose forward in time using linear field-frame velocity components.
+   *
+   * <p>The robot's field-frame position advances by {@code vx * dt} and {@code vy * dt}. Heading
+   * advances by {@code omega * dt}. This avoids the curvature error introduced by {@link
+   * Pose2d#exp(Twist2d)} because the drive code already compensates for curvature when generating
+   * robot-relative motor commands (via {@code ChassisSpeeds.discretize}).
+   */
+  private static Pose2d linearExtrapolatePose(
+      Pose2d currentPose, ChassisSpeeds fieldVelocity, double dt) {
+    Translation2d newTranslation =
+        currentPose
+            .getTranslation()
+            .plus(
+                new Translation2d(
+                    fieldVelocity.vxMetersPerSecond * dt, fieldVelocity.vyMetersPerSecond * dt));
+    Rotation2d newRotation =
+        currentPose
+            .getRotation()
+            .plus(Rotation2d.fromRadians(fieldVelocity.omegaRadiansPerSecond * dt));
+    return new Pose2d(newTranslation, newRotation);
+  }
+
+  public void clearShootingParameters() {
+    latestParameters = null;
+  }
+
+  private TurretControlPhysics getTurretControlPhysics(Translation2d turretOffset) {
+    if (turretControlPhysics == null
+        || cachedTurretOffset == null
+        || !cachedTurretOffset.equals(turretOffset)) {
+      cachedTurretOffset = turretOffset;
+      turretControlPhysics =
+          new TurretControlPhysics(
+              turretOffset,
+              minTurretAngle,
+              maxTurretAngle,
+              feedforwardPaddingAngle,
+              settlingGain,
+              this::getTimeOfFlightSeconds,
+              settlingTimeFunction,
+              minDistance,
+              maxDistance);
+    }
+    return turretControlPhysics;
+  }
+
+  private double getTimeOfFlightSeconds(double distanceMeters) {
+    Double time = timeOfFlightMap.get(distanceMeters);
+    return time != null ? time : 0.0;
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretConfig.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretConfig.java
new file mode 100644
index 00000000..0b600948
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretConfig.java
@@ -0,0 +1,322 @@
+package frc.robot.rebuilt.subsystems.Launcher;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+import yams.motorcontrollers.SmartMotorController;
+
+/**
+ * Configuration for the {@link SmartTurretController} 2-state turret control system.
+ *
+ * <p>All slot feedforward values (kS, kV, kA) are in <b>Amps</b> for use with TorqueCurrentFOC
+ * control. The expo profile parameters (expoKV, expoKA) are always in <b>Volts</b> regardless of
+ * control mode. All positional values are in <b>mechanism rotations</b> (post-gear-reduction).
+ */
+public class SmartTurretConfig {
+
+  // Hardware
+  private final TalonFX talonFX;
+  /**
+   * YAMS SmartMotorController — its closed-loop Notifier will be stopped when SmartTurretController
+   * takes over.
+   */
+  private final SmartMotorController yamsController;
+
+  private final double gearRatio;
+
+  // Motion constraints (mechanism rot/s and rot/s^2)
+  private final double maxVelocityMechRotPerSec;
+  private final double maxAccelMechRotPerSecSq;
+
+  // Seeking PID (Slot0 — MotionMagicTorqueCurrentFOC)
+  private final double seekingKP;
+  private final double seekingKI;
+  private final double seekingKD;
+
+  // Tracking PID (Slot1 — PositionTorqueCurrentFOC)
+  private final double trackingKP;
+  private final double trackingKI;
+  private final double trackingKD;
+
+  // Feedforward in Amps (TorqueCurrentFOC units)
+  private final double kS;
+  private final double kV;
+  private final double kA;
+
+  // MotionMagicExpo plant model — ALWAYS in Volts regardless of control mode.
+  // These define the mechanism's voltage-domain model for the expo velocity profile.
+  private final double expoKV; // V/(mechanism rot/s)
+  private final double expoKA; // V/(mechanism rot/s^2)
+
+  // State transition thresholds (mechanism rotations)
+  private final double seekingThresholdRotations;
+  private final double hysteresisBufferRotations;
+
+  // Soft limits (mechanism rotations)
+  private final double lowerLimitRotations;
+  private final double upperLimitRotations;
+
+  // Peak torque current limit (Amps)
+  private final double peakTorqueCurrentAmps;
+
+  // Feedforward safety padding near limits (mechanism rotations)
+  private final double feedforwardPaddingRotations;
+
+  // Tracking-mode kS deadband (mechanism rotations).
+  // When |positionError| < this value, external kS feedforward is zeroed to prevent chattering.
+  private final double trackingDeadbandRotations;
+
+  private SmartTurretConfig(Builder builder) {
+    this.talonFX = builder.talonFX;
+    this.yamsController = builder.yamsController;
+    this.gearRatio = builder.gearRatio;
+    this.maxVelocityMechRotPerSec = builder.maxVelocityMechRotPerSec;
+    this.maxAccelMechRotPerSecSq = builder.maxAccelMechRotPerSecSq;
+    this.seekingKP = builder.seekingKP;
+    this.seekingKI = builder.seekingKI;
+    this.seekingKD = builder.seekingKD;
+    this.trackingKP = builder.trackingKP;
+    this.trackingKI = builder.trackingKI;
+    this.trackingKD = builder.trackingKD;
+    this.kS = builder.kS;
+    this.kV = builder.kV;
+    this.kA = builder.kA;
+    this.expoKV = builder.expoKV;
+    this.expoKA = builder.expoKA;
+    this.seekingThresholdRotations = builder.seekingThresholdRotations;
+    this.hysteresisBufferRotations = builder.hysteresisBufferRotations;
+    this.lowerLimitRotations = builder.lowerLimitRotations;
+    this.upperLimitRotations = builder.upperLimitRotations;
+    this.peakTorqueCurrentAmps = builder.peakTorqueCurrentAmps;
+    this.feedforwardPaddingRotations = builder.feedforwardPaddingRotations;
+    this.trackingDeadbandRotations = builder.trackingDeadbandRotations;
+  }
+
+  public TalonFX getTalonFX() {
+    return talonFX;
+  }
+
+  /** Returns the YAMS SmartMotorController, or {@code null} if not configured. */
+  public SmartMotorController getYamsController() {
+    return yamsController;
+  }
+
+  public double getGearRatio() {
+    return gearRatio;
+  }
+
+  public double getMaxVelocityMechRotPerSec() {
+    return maxVelocityMechRotPerSec;
+  }
+
+  public double getMaxAccelMechRotPerSecSq() {
+    return maxAccelMechRotPerSecSq;
+  }
+
+  public double getSeekingKP() {
+    return seekingKP;
+  }
+
+  public double getSeekingKI() {
+    return seekingKI;
+  }
+
+  public double getSeekingKD() {
+    return seekingKD;
+  }
+
+  public double getTrackingKP() {
+    return trackingKP;
+  }
+
+  public double getTrackingKI() {
+    return trackingKI;
+  }
+
+  public double getTrackingKD() {
+    return trackingKD;
+  }
+
+  public double getKS() {
+    return kS;
+  }
+
+  public double getKV() {
+    return kV;
+  }
+
+  public double getKA() {
+    return kA;
+  }
+
+  /** MotionMagicExpo kV in V/(mechanism rot/s). Always in Volts regardless of control mode. */
+  public double getExpoKV() {
+    return expoKV;
+  }
+
+  /** MotionMagicExpo kA in V/(mechanism rot/s^2). Always in Volts regardless of control mode. */
+  public double getExpoKA() {
+    return expoKA;
+  }
+
+  public double getSeekingThresholdRotations() {
+    return seekingThresholdRotations;
+  }
+
+  public double getHysteresisBufferRotations() {
+    return hysteresisBufferRotations;
+  }
+
+  public double getLowerLimitRotations() {
+    return lowerLimitRotations;
+  }
+
+  public double getUpperLimitRotations() {
+    return upperLimitRotations;
+  }
+
+  public double getPeakTorqueCurrentAmps() {
+    return peakTorqueCurrentAmps;
+  }
+
+  public double getFeedforwardPaddingRotations() {
+    return feedforwardPaddingRotations;
+  }
+
+  public double getTrackingDeadbandRotations() {
+    return trackingDeadbandRotations;
+  }
+
+  public static class Builder {
+    private TalonFX talonFX;
+    private SmartMotorController yamsController = null;
+    private double gearRatio = 30.0;
+    private double maxVelocityMechRotPerSec = 3.0;
+    private double maxAccelMechRotPerSecSq = 2.78;
+    private double seekingKP = 225;
+    private double seekingKI = 0;
+    private double seekingKD = 50;
+    private double trackingKP = 225;
+    private double trackingKI = 0;
+    private double trackingKD = 50;
+    private double kS = 10.0;
+    private double kV = 0.0;
+    private double kA = 5.0;
+    // Expo defaults: computed from motion constraints in build() if not explicitly set.
+    // -1 signals "auto-compute from maxVelocity/maxAccel".
+    private double expoKV = -1;
+    private double expoKA = -1;
+    private double seekingThresholdRotations = 10.0 / 360.0;
+    private double hysteresisBufferRotations = 3.0 / 360.0;
+    private double lowerLimitRotations = -150.0 / 360.0;
+    private double upperLimitRotations = 150.0 / 360.0;
+    private double peakTorqueCurrentAmps = 240.0;
+    private double feedforwardPaddingRotations = 10.0 / 360.0; // 10 degrees
+    private double trackingDeadbandRotations = 0.25 / 360.0; // 0.25 degrees
+
+    public Builder withTalonFX(TalonFX talonFX) {
+      this.talonFX = talonFX;
+      return this;
+    }
+
+    /**
+     * Provides the YAMS {@link SmartMotorController} so {@link SmartTurretController} can stop its
+     * background closed-loop Notifier thread on construction, preventing interference with our
+     * direct TorqueCurrentFOC control.
+     */
+    public Builder withYAMSController(SmartMotorController yamsController) {
+      this.yamsController = yamsController;
+      return this;
+    }
+
+    public Builder withGearRatio(double gearRatio) {
+      this.gearRatio = gearRatio;
+      return this;
+    }
+
+    public Builder withMotionConstraints(
+        double maxVelocityMechRotPerSec, double maxAccelMechRotPerSecSq) {
+      this.maxVelocityMechRotPerSec = maxVelocityMechRotPerSec;
+      this.maxAccelMechRotPerSecSq = maxAccelMechRotPerSecSq;
+      return this;
+    }
+
+    public Builder withSeekingPID(double kP, double kI, double kD) {
+      this.seekingKP = kP;
+      this.seekingKI = kI;
+      this.seekingKD = kD;
+      return this;
+    }
+
+    public Builder withTrackingPID(double kP, double kI, double kD) {
+      this.trackingKP = kP;
+      this.trackingKI = kI;
+      this.trackingKD = kD;
+      return this;
+    }
+
+    public Builder withFeedforward(double kS, double kV, double kA) {
+      this.kS = kS;
+      this.kV = kV;
+      this.kA = kA;
+      return this;
+    }
+
+    /**
+     * Sets the MotionMagicExpo plant model parameters in Volts. These are always in V/rps and
+     * V/rps^2 regardless of control mode (even when using TorqueCurrentFOC).
+     *
+     * <p>If not called, defaults are auto-computed from motion constraints as {@code 12.0 /
+     * maxVelocity} and {@code 12.0 / maxAcceleration}.
+     */
+    public Builder withExpoConstraints(double expoKV, double expoKA) {
+      this.expoKV = expoKV;
+      this.expoKA = expoKA;
+      return this;
+    }
+
+    public Builder withSeekingThreshold(double seekingThresholdRotations) {
+      this.seekingThresholdRotations = seekingThresholdRotations;
+      return this;
+    }
+
+    public Builder withHysteresisBuffer(double hysteresisBufferRotations) {
+      this.hysteresisBufferRotations = hysteresisBufferRotations;
+      return this;
+    }
+
+    public Builder withSoftLimits(double lowerLimitRotations, double upperLimitRotations) {
+      this.lowerLimitRotations = lowerLimitRotations;
+      this.upperLimitRotations = upperLimitRotations;
+      return this;
+    }
+
+    public Builder withPeakTorqueCurrent(double peakAmps) {
+      this.peakTorqueCurrentAmps = peakAmps;
+      return this;
+    }
+
+    public Builder withFeedforwardPadding(double feedforwardPaddingRotations) {
+      this.feedforwardPaddingRotations = feedforwardPaddingRotations;
+      return this;
+    }
+
+    public Builder withTrackingDeadband(double trackingDeadbandRotations) {
+      this.trackingDeadbandRotations = trackingDeadbandRotations;
+      return this;
+    }
+
+    public SmartTurretConfig build() {
+      if (talonFX == null) {
+        throw new IllegalStateException("TalonFX must be set");
+      }
+      // Auto-compute expo plant model from motion constraints if not explicitly set.
+      // V_max / kV_expo = max achievable velocity; V_max / kA_expo = max achievable acceleration.
+      if (expoKV < 0) {
+        expoKV = 12.0 / maxVelocityMechRotPerSec;
+      }
+      if (expoKA < 0) {
+        expoKA = 12.0 / maxAccelMechRotPerSecSq;
+      }
+      return new SmartTurretConfig(this);
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretController.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretController.java
new file mode 100644
index 00000000..6ed256be
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretController.java
@@ -0,0 +1,397 @@
+package frc.robot.rebuilt.subsystems.Launcher;
+
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.ParentDevice;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import java.util.concurrent.atomic.AtomicReference;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * 2-state turret controller that switches between SEEKING and TRACKING modes.
+ *
+ * <p><b>SEEKING</b>: Uses {@link MotionMagicExpoTorqueCurrentFOC} (TalonFX exponential profiling)
+ * for smooth, physically-optimal travel over large distances. The exponential profile adapts to the
+ * mechanism's motor model (kV/kA) for crisp arrivals without overshoot. Velocity is capped by
+ * {@code MotionMagicCruiseVelocity}.
+ *
+ * <p><b>TRACKING</b>: Uses {@link PositionTorqueCurrentFOC} with explicit velocity and acceleration
+ * feedforward to smoothly track a moving target at close range.
+ *
+ * <p>State transitions use hysteresis to prevent jitter:
+ *
+ * <ul>
+ *   <li>SEEKING → TRACKING when |error| &lt; seekingThreshold
+ *   <li>TRACKING → SEEKING when |error| &gt; seekingThreshold + hysteresisBuffer
+ * </ul>
+ *
+ * <p>The {@link #step(double)} method is called at 200 Hz via Notifier, while {@link #setTarget} is
+ * called from the 20 ms robot loop. Thread safety is ensured via {@link AtomicReference} over an
+ * immutable {@link TurretTarget} record.
+ */
+public class SmartTurretController {
+
+  /** The two operational states of the turret controller. */
+  public enum TurretState {
+    SEEKING,
+    TRACKING
+  }
+
+  /** Immutable target data published from the 20 ms loop and consumed by the 200 Hz step loop. */
+  public record TurretTarget(
+      double positionMechRot, double velocityRadPerSec, double accelerationRadPerSecSq) {}
+
+  /** Signal update frequency for turret telemetry on CANivore (Hz). */
+  private static final double SIGNAL_UPDATE_FREQUENCY_HZ = 250.0;
+
+  private final TalonFX talonFX;
+  private final SmartTurretConfig config;
+
+  // High-frequency status signals — cached references for refreshAll() in characterization.
+  private final StatusSignal<Angle> positionSignal;
+  private final StatusSignal<AngularVelocity> velocitySignal;
+  private final StatusSignal<AngularAcceleration> accelerationSignal;
+  private final StatusSignal<Current> torqueCurrentSignal;
+
+  // Pre-allocated control requests (reused each cycle to avoid allocation).
+  private final MotionMagicExpoTorqueCurrentFOC seekingRequest =
+      new MotionMagicExpoTorqueCurrentFOC(0).withSlot(0);
+  private final PositionTorqueCurrentFOC trackingRequest =
+      new PositionTorqueCurrentFOC(0).withSlot(1);
+
+  // Thread-safe target from 20 ms loop to 200 Hz step loop.
+  private final AtomicReference<TurretTarget> target =
+      new AtomicReference<>(new TurretTarget(0, 0, 0));
+
+  private volatile TurretState currentState = TurretState.SEEKING;
+  private volatile boolean enabled = false;
+  private volatile boolean forceDisabled = false;
+
+  /**
+   * Constructs a SmartTurretController and configures the TalonFX with two PID slots, MotionMagic
+   * parameters, and torque current limits.
+   *
+   * <p>Uses read-modify-write on the TalonFX configuration to preserve YAMS-set fields (inversion,
+   * neutral mode, sensor ratio, etc.).
+   */
+  public SmartTurretController(SmartTurretConfig config) {
+    this.config = config;
+    this.talonFX = config.getTalonFX();
+
+    // Read existing config to preserve YAMS-applied settings.
+    TalonFXConfiguration fxConfig = new TalonFXConfiguration();
+    talonFX.getConfigurator().refresh(fxConfig);
+
+    // Slot0: Seeking (MotionMagicExpoTorqueCurrentFOC)
+    fxConfig.Slot0.kP = config.getSeekingKP();
+    fxConfig.Slot0.kI = config.getSeekingKI();
+    fxConfig.Slot0.kD = config.getSeekingKD();
+    fxConfig.Slot0.kS = config.getKS();
+    fxConfig.Slot0.kV = config.getKV();
+    fxConfig.Slot0.kA = config.getKA();
+
+    // Slot1: Tracking (PositionTorqueCurrentFOC)
+    // kV and kA run at firmware frequency (1 kHz) for the best temporal alignment with PID.
+    // The Velocity field on the control request provides the reference for kV.
+    // kS is set to 0 in the slot because it is position-dependent and injected externally
+    // via withFeedForward() each cycle. This prevents chattering from velocity-sign-based kS
+    // flipping at near-zero velocity.
+    fxConfig.Slot1.kP = config.getTrackingKP();
+    fxConfig.Slot1.kI = config.getTrackingKI();
+    fxConfig.Slot1.kD = config.getTrackingKD();
+    fxConfig.Slot1.kS = 0;
+    fxConfig.Slot1.kV = config.getKV();
+    fxConfig.Slot1.kA = config.getKA();
+
+    // MotionMagicExpo: voltage-domain plant model for the exponential velocity profile.
+    // These are ALWAYS in Volts (V/rps and V/rps²) regardless of control output type.
+    // They define the mechanism's physical limits so the expo profile shapes appropriately.
+    // CruiseVelocity provides an additional hard cap on profile velocity.
+    fxConfig.MotionMagic.MotionMagicExpo_kV = config.getExpoKV();
+    fxConfig.MotionMagic.MotionMagicExpo_kA = config.getExpoKA();
+    fxConfig.MotionMagic.MotionMagicCruiseVelocity = config.getMaxVelocityMechRotPerSec();
+
+    // Torque current peak limits.
+    fxConfig.TorqueCurrent.PeakForwardTorqueCurrent = config.getPeakTorqueCurrentAmps();
+    fxConfig.TorqueCurrent.PeakReverseTorqueCurrent = -config.getPeakTorqueCurrentAmps();
+    fxConfig.CurrentLimits.StatorCurrentLimitEnable = false;
+
+    // Firmware-level soft limits — last line of defense against hard stop contact.
+    fxConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+    fxConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = config.getUpperLimitRotations();
+    fxConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+    fxConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = config.getLowerLimitRotations();
+
+    talonFX.getConfigurator().apply(fxConfig);
+
+    // Cache high-frequency status signals for position, velocity, and torque current.
+    // Set them to 250 Hz on CANivore for smooth characterization data.
+    positionSignal = talonFX.getPosition();
+    velocitySignal = talonFX.getVelocity();
+    accelerationSignal = talonFX.getAcceleration();
+    torqueCurrentSignal = talonFX.getTorqueCurrent();
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        SIGNAL_UPDATE_FREQUENCY_HZ,
+        positionSignal,
+        velocitySignal,
+        accelerationSignal,
+        torqueCurrentSignal);
+    ParentDevice.optimizeBusUtilizationForAll(talonFX);
+
+    // Stop the YAMS SmartMotorController's background closed-loop Notifier. YAMS runs its own
+    // 20ms update loop that continuously sends position setpoints to the TalonFX. Since
+    // SmartTurretController now owns this motor via direct TorqueCurrentFOC commands, that loop
+    // must be permanently disabled to prevent interference.
+    if (config.getYamsController() != null) {
+      config.getYamsController().stopClosedLoopController();
+    }
+  }
+
+  /**
+   * Sets the turret target from the 20 ms robot loop.
+   *
+   * @param position desired turret mechanism angle
+   * @param velocityRadPerSec angular velocity feedforward in rad/s (mechanism units)
+   * @param accelerationRadPerSecSq angular acceleration feedforward in rad/s^2 (mechanism units)
+   */
+  public void setTarget(Angle position, double velocityRadPerSec, double accelerationRadPerSecSq) {
+    double positionMechRot = position.in(edu.wpi.first.units.Units.Rotations);
+    positionMechRot =
+        MathUtil.clamp(
+            positionMechRot, config.getLowerLimitRotations(), config.getUpperLimitRotations());
+    target.set(new TurretTarget(positionMechRot, velocityRadPerSec, accelerationRadPerSecSq));
+    enabled = true;
+
+    Logger.recordOutput("SmartTurret/GoalPosition", positionMechRot);
+  }
+
+  /**
+   * Steps the controller at 200 Hz. Determines the current state and applies the appropriate
+   * control mode to the TalonFX.
+   *
+   * @param dtSeconds the time step (unused in onboard-profiling mode, retained for interface
+   *     compatibility)
+   */
+  public void step(double dtSeconds) {
+    if (!enabled || forceDisabled) {
+      return;
+    }
+
+    TurretTarget currentTarget = target.get();
+    double actualPositionMechRot = talonFX.getPosition().getValueAsDouble();
+    double positionError = Math.abs(currentTarget.positionMechRot() - actualPositionMechRot);
+
+    // State transition with hysteresis.
+    if (currentState == TurretState.SEEKING) {
+      if (positionError < config.getSeekingThresholdRotations()) {
+        currentState = TurretState.TRACKING;
+      }
+    } else { // TRACKING
+      if (positionError
+          > config.getSeekingThresholdRotations() + config.getHysteresisBufferRotations()) {
+        currentState = TurretState.SEEKING;
+      }
+    }
+
+    switch (currentState) {
+      case SEEKING:
+        talonFX.setControl(seekingRequest.withPosition(currentTarget.positionMechRot()));
+        break;
+
+      case TRACKING:
+        // Compute position-error-directed kS feedforward externally.
+        // Unlike firmware kS (which keys on velocity sign and chatters at zero),
+        // this keys on position-error sign so it always pushes toward the target.
+        // Inside the deadband, kS is zeroed to let the turret settle without oscillation.
+        double signedError = currentTarget.positionMechRot() - actualPositionMechRot;
+        double ksFeedforward;
+        if (positionError < config.getTrackingDeadbandRotations()) {
+          ksFeedforward = 0.0;
+        } else {
+          ksFeedforward = Math.signum(signedError) * config.getKS();
+        }
+
+        talonFX.setControl(
+            trackingRequest
+                .withPosition(currentTarget.positionMechRot())
+                .withVelocity(RadiansPerSecond.of(currentTarget.velocityRadPerSec))
+                .withFeedForward(ksFeedforward));
+
+        break;
+    }
+
+    // Logging.
+    Logger.recordOutput("SmartTurret/State", currentState.name());
+    Logger.recordOutput("SmartTurret/PositionErrorRot", positionError);
+    Logger.recordOutput("SmartTurret/ActualPositionMechRot", actualPositionMechRot);
+    Logger.recordOutput("SmartTurret/TargetPositionMechRot", currentTarget.positionMechRot());
+  }
+
+  /**
+   * Scales feedforward toward zero when the turret is near a soft limit to prevent hard stop
+   * contact. The feedforward is linearly ramped down within the padding zone.
+   */
+  private double applyFeedforwardSafetyPadding(double positionMechRot, double feedforward) {
+    double padding = config.getFeedforwardPaddingRotations();
+    double upper = config.getUpperLimitRotations();
+    double lower = config.getLowerLimitRotations();
+
+    if (feedforward > 0 && positionMechRot > (upper - padding)) {
+      double scale = MathUtil.clamp((upper - positionMechRot) / padding, 0.0, 1.0);
+      return feedforward * scale;
+    }
+    if (feedforward < 0 && positionMechRot < (lower + padding)) {
+      double scale = MathUtil.clamp((positionMechRot - lower) / padding, 0.0, 1.0);
+      return feedforward * scale;
+    }
+    return feedforward;
+  }
+
+  /**
+   * Estimates the time for the turret to arrive at the given goal position from its current state.
+   *
+   * <p>If the error is within the tracking threshold, returns a small constant. Otherwise, uses the
+   * closed-form trapezoidal settling time for the seeking portion plus tracking settle time.
+   *
+   * @param goalPositionMechRot goal position in mechanism rotations
+   * @return estimated time to arrival in seconds
+   */
+  public double getEstimatedTimeToArrival(double goalPositionMechRot) {
+    double actualMechRot = talonFX.getPosition().getValueAsDouble();
+    double errorRad = Math.abs(goalPositionMechRot - actualMechRot) * 2.0 * Math.PI;
+    double velocityRadPerSec = getActualVelocityRadPerSec();
+    double vMaxRad = config.getMaxVelocityMechRotPerSec() * 2.0 * Math.PI;
+    double aMaxRad = config.getMaxAccelMechRotPerSecSq() * 2.0 * Math.PI;
+    double seekingThresholdRad = config.getSeekingThresholdRotations() * 2.0 * Math.PI;
+
+    // Small constant for tracking-mode settling time.
+    double trackingSettleTime = 0.05;
+
+    if (errorRad <= seekingThresholdRad) {
+      return trackingSettleTime;
+    }
+
+    // Seeking time for the distance beyond the tracking threshold.
+    double seekError = errorRad - seekingThresholdRad;
+    double seekTime =
+        TurretControlPhysics.trapezoidSettlingTime(seekError, velocityRadPerSec, vMaxRad, aMaxRad);
+    return seekTime + trackingSettleTime;
+  }
+
+  /**
+   * Returns the actual motor encoder velocity in mechanism rad/s.
+   *
+   * @return actual motor velocity in rad/s (mechanism units)
+   */
+  public double getActualVelocityRadPerSec() {
+    return talonFX.getVelocity().getValueAsDouble() * 2.0 * Math.PI;
+  }
+
+  /**
+   * Returns the actual motor encoder position in mechanism rotations.
+   *
+   * @return actual motor position in mechanism rotations
+   */
+  public double getActualPositionMechRot() {
+    return talonFX.getPosition().getValueAsDouble();
+  }
+
+  /**
+   * Returns the current goal position in mechanism rotations.
+   *
+   * @return goal mechanism position in rotations
+   */
+  public double getGoalPositionMechRot() {
+    return target.get().positionMechRot();
+  }
+
+  /**
+   * Returns the current turret state (SEEKING or TRACKING).
+   *
+   * @return current {@link TurretState}
+   */
+  public TurretState getCurrentTurretState() {
+    return currentState;
+  }
+
+  /** Stops the turret by disabling the controller. The TalonFX will hold its last command. */
+  public void stop() {
+    enabled = false;
+    double actualMechRot = talonFX.getPosition().getValueAsDouble();
+    target.set(new TurretTarget(actualMechRot, 0, 0));
+    currentState = TurretState.SEEKING;
+  }
+
+  /**
+   * Resets the controller state. Use during robot init before encoder readings are available.
+   *
+   * @param currentPositionMechRot mechanism position in rotations to seed
+   * @param currentVelocityMechRotPerSec mechanism velocity in rot/s to seed (unused, retained for
+   *     API compatibility)
+   */
+  public void reset(double currentPositionMechRot, double currentVelocityMechRotPerSec) {
+    target.set(new TurretTarget(currentPositionMechRot, 0, 0));
+    currentState = TurretState.SEEKING;
+    enabled = false;
+  }
+
+  /**
+   * Returns the underlying TalonFX for use by tuning commands.
+   *
+   * @return the TalonFX hardware reference
+   */
+  public TalonFX getTalonFX() {
+    return talonFX;
+  }
+
+  /**
+   * Returns the high-frequency position status signal (250 Hz on CANivore). Use with {@link
+   * BaseStatusSignal#refreshAll} for latency-compensated reads.
+   */
+  public StatusSignal<Angle> getPositionSignal() {
+    return positionSignal;
+  }
+
+  /**
+   * Returns the high-frequency velocity status signal (250 Hz on CANivore). Use with {@link
+   * BaseStatusSignal#refreshAll} for latency-compensated reads.
+   */
+  public StatusSignal<AngularVelocity> getVelocitySignal() {
+    return velocitySignal;
+  }
+
+  /**
+   * Returns the high-frequency torque current status signal (250 Hz on CANivore). Use with {@link
+   * BaseStatusSignal#refreshAll} for latency-compensated reads.
+   */
+  public StatusSignal<Current> getTorqueCurrentSignal() {
+    return torqueCurrentSignal;
+  }
+
+  /**
+   * Returns the high-frequency acceleration status signal (250 Hz on CANivore). Use with {@link
+   * BaseStatusSignal#refreshAll} for latency-compensated reads.
+   */
+  public StatusSignal<AngularAcceleration> getAccelerationSignal() {
+    return accelerationSignal;
+  }
+
+  /**
+   * Returns the configuration used to construct this controller.
+   *
+   * @return the {@link SmartTurretConfig}
+   */
+  public SmartTurretConfig getConfig() {
+    return config;
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/TurretControlPhysics.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/TurretControlPhysics.java
new file mode 100644
index 00000000..beba1e49
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/TurretControlPhysics.java
@@ -0,0 +1,543 @@
+package frc.robot.rebuilt.subsystems.Launcher;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.function.BiFunction;
+import java.util.function.DoubleFunction;
+
+public class TurretControlPhysics {
+  /** Defines configuration parameters and physical constraints for turret aiming calculations */
+  private final Translation2d turretOffsetRobotFrame;
+
+  private final Rotation2d minTurretAngle;
+  private final Rotation2d maxTurretAngle;
+  private final Rotation2d feedforwardPaddingAngle;
+  private final double settlingTimeGain; // Alpha filter gain (0.0 to 1.0)
+
+  private static final double DERIVATIVE_PROBE_TIME_DELTA = 0.005; // 5ms
+  private static final int MAX_SOLVER_ITERATIONS = 4;
+  private static final double CONVERGENCE_THRESHOLD_SECONDS = 0.001;
+
+  private final DoubleFunction<Double> timeOfFlightFunction;
+  private final BiFunction<Double, Double, Double> settlingTimeFunction;
+  private final double minEffectiveRangeMeters;
+  private final double maxEffectiveRangeMeters;
+
+  /**
+   * Builds a settling-time function from trapezoidal motion-profile constraints.
+   *
+   * <p>Given angle error {@code |Δθ|} in radians the function returns the time (seconds) for the
+   * turret to reach its goal under a trapezoidal velocity profile with peak velocity {@code vMax}
+   * (rad/s) and peak acceleration {@code aMax} (rad/s²).
+   *
+   * <ul>
+   *   <li>Triangle phase: {@code t = 2 * sqrt(|Δθ| / aMax)} when {@code |Δθ| < vMax²/aMax}
+   *   <li>Trapezoid phase: {@code t = vMax/aMax + |Δθ|/vMax} otherwise
+   * </ul>
+   *
+   * @param maxVelocityRadPerSec peak turret velocity (rad/s)
+   * @param maxAccelRadPerSecSq peak turret acceleration (rad/s²)
+   * @return a {@link DoubleFunction} mapping |angleErrorRadians| → settlingTimeSeconds
+   */
+  public static DoubleFunction<Double> trapezoidalSettlingTimeFunction(
+      double maxVelocityRadPerSec, double maxAccelRadPerSecSq) {
+    double vMax = Math.abs(maxVelocityRadPerSec);
+    double aMax = Math.abs(maxAccelRadPerSecSq);
+    if (vMax < 1e-6 || aMax < 1e-6) {
+      return (err) -> 0.0;
+    }
+    double triangleThreshold = (vMax * vMax) / aMax;
+    return (angleErrorRad) -> {
+      double err = Math.abs(angleErrorRad);
+      if (err < triangleThreshold) {
+        return 2.0 * Math.sqrt(err / aMax);
+      } else {
+        return vMax / aMax + err / vMax;
+      }
+    };
+  }
+
+  /**
+   * Builds a velocity-aware settling-time function from trapezoidal motion-profile constraints.
+   *
+   * <p>Unlike {@link #trapezoidalSettlingTimeFunction}, this version accounts for the turret's
+   * current velocity when estimating time-to-arrival. Uses a closed-form analytical formula — O(1)
+   * cost with a single sqrt — derived from the three-phase trapezoidal profile:
+   *
+   * <ul>
+   *   <li>Moving toward target without overshoot: normal accel-cruise-decel formula
+   *   <li>Moving toward target but overshooting: decelerate past goal, then come back from rest
+   *   <li>Moving away from target: decelerate to stop (going backward), then move forward
+   * </ul>
+   *
+   * <p>The returned estimate is accurate to within the settlingTimeGain that the caller applies;
+   * the overshoot case uses a symmetric-return path which very slightly undershoots reality.
+   *
+   * @param maxVelocityRadPerSec peak turret velocity (rad/s)
+   * @param maxAccelRadPerSecSq peak turret acceleration (rad/s²)
+   * @return a {@link BiFunction} mapping (|angleErrorRadians|, currentVelocityRadPerSec) →
+   *     settlingTimeSeconds
+   */
+  public static BiFunction<Double, Double, Double> velocityAwareSettlingTimeFunction(
+      double maxVelocityRadPerSec, double maxAccelRadPerSecSq) {
+    double vMax = Math.abs(maxVelocityRadPerSec);
+    double aMax = Math.abs(maxAccelRadPerSecSq);
+    if (vMax < 1e-6 || aMax < 1e-6) {
+      return (err, vel) -> 0.0;
+    }
+    return (angleErrorRad, currentVelocityRadPerSec) ->
+        trapezoidSettlingTime(Math.abs(angleErrorRad), currentVelocityRadPerSec, vMax, aMax);
+  }
+
+  /**
+   * Builds a settling-time function that accounts for the 2-state SmartTurretController behavior.
+   *
+   * <p>When the turret is already within the seeking threshold (close to target), only a small
+   * constant tracking settle time is needed. When farther away, the trapezoidal settling time is
+   * used for the seeking portion beyond the threshold, plus the tracking settle time.
+   *
+   * @param seekingThresholdRad the SEEKING -> TRACKING transition threshold in radians
+   * @param trackingSettleTimeSeconds constant settle time for the tracking-mode PID (~0.05s)
+   * @param maxVelocityRadPerSec peak turret velocity (rad/s)
+   * @param maxAccelRadPerSecSq peak turret acceleration (rad/s^2)
+   * @return a {@link BiFunction} mapping (|angleErrorRadians|, currentVelocityRadPerSec) ->
+   *     settlingTimeSeconds
+   */
+  public static BiFunction<Double, Double, Double> twoStateSettlingTimeFunction(
+      double seekingThresholdRad,
+      double trackingSettleTimeSeconds,
+      double maxVelocityRadPerSec,
+      double maxAccelRadPerSecSq) {
+    double vMax = Math.abs(maxVelocityRadPerSec);
+    double aMax = Math.abs(maxAccelRadPerSecSq);
+    if (vMax < 1e-6 || aMax < 1e-6) {
+      return (err, vel) -> 0.0;
+    }
+    return (angleErrorRad, currentVelocityRadPerSec) -> {
+      double absError = Math.abs(angleErrorRad);
+      if (absError <= seekingThresholdRad) {
+        return trackingSettleTimeSeconds;
+      }
+      double seekError = absError - seekingThresholdRad;
+      double seekTime = trapezoidSettlingTime(seekError, currentVelocityRadPerSec, vMax, aMax);
+      return seekTime + trackingSettleTimeSeconds;
+    };
+  }
+
+  /**
+   * Closed-form trapezoidal settling-time estimate.
+   *
+   * @param d absolute angle error in rad (must be ≥ 0)
+   * @param v0 current velocity in rad/s (positive = toward target)
+   * @param vMax peak velocity in rad/s
+   * @param aMax peak acceleration in rad/s²
+   * @return estimated settling time in seconds
+   */
+  static double trapezoidSettlingTime(double d, double v0, double vMax, double aMax) {
+    if (d < 1e-9) return 0.0;
+    // Clamp to reachable velocity range.
+    v0 = Math.max(-vMax, Math.min(vMax, v0));
+
+    if (v0 < 0.0) {
+      // Moving away from the target. First decelerate to rest (while traveling backward),
+      // then cover the original distance plus the backward overshoot from rest.
+      double tDecel = -v0 / aMax; // > 0
+      double dBack = v0 * v0 / (2.0 * aMax); // distance traveled backward
+      return tDecel + fromRest(d + dBack, vMax, aMax);
+    }
+
+    // v0 >= 0: moving toward the target.
+    double dStop = v0 * v0 / (2.0 * aMax); // braking distance from v0 to 0
+    if (dStop > d) {
+      // Would overshoot. Decelerate past the target, stop, then return from rest.
+      double tDecel = v0 / aMax;
+      double dOvershoot = dStop - d; // how far past the target before stopping
+      return tDecel + fromRest(dOvershoot, vMax, aMax);
+    }
+
+    // Normal approach: won't overshoot. Check trapezoid vs triangle.
+    double d1 = (vMax * vMax - v0 * v0) / (2.0 * aMax); // to accelerate from v0 to vMax
+    double d3 = vMax * vMax / (2.0 * aMax); // to decelerate from vMax to 0
+    if (d >= d1 + d3) {
+      // Trapezoid: accelerate to vMax, cruise, decelerate to 0.
+      double t1 = (vMax - v0) / aMax;
+      double t2 = (d - d1 - d3) / vMax;
+      double t3 = vMax / aMax;
+      return t1 + t2 + t3;
+    } else {
+      // Triangle: accelerate to v_peak, decelerate to 0. v_peak < vMax.
+      double vPeak = Math.sqrt(aMax * d + v0 * v0 / 2.0);
+      return (vPeak - v0) / aMax + vPeak / aMax;
+    }
+  }
+
+  /** Settling time from rest (v=0) to cover distance d under a trapezoidal profile. */
+  private static double fromRest(double d, double vMax, double aMax) {
+    double threshold = vMax * vMax / aMax;
+    if (d < threshold) {
+      return 2.0 * Math.sqrt(d / aMax); // triangle
+    } else {
+      return vMax / aMax + d / vMax; // trapezoid
+    }
+  }
+
+  public record RobotState(Pose2d pose, ChassisSpeeds velocity, ChassisSpeeds acceleration) {}
+
+  @FunctionalInterface
+  public interface RobotPredictor {
+    RobotState predict(double timeSinceStartSeconds, double lookaheadSeconds);
+  }
+
+  /**
+   * @param turretOffsetRobotFrame Vector from robot center to turret center (Robot Frame).
+   * @param minTurretAngle Minimum physical rotation limit (e.g. -165 deg).
+   * @param maxTurretAngle Maximum physical rotation limit (e.g. +165 deg).
+   * @param feedforwardPaddingAngle Buffer zone near limits where velocity is ramped down.
+   * @param settlingGain Gain for settling time estimation (<1.0 underestimates to prevent
+   *     oscillation).
+   * @param timeOfFlightFunc Function returning projectile flight time (s) given distance (m).
+   * @param settlingTimeFunc Function returning turret settling time (s) given angle error (rad) and
+   *     current velocity (rad/s).
+   * @param minRangeMeters Minimum effective shot range.
+   * @param maxRangeMeters Maximum effective shot range.
+   */
+  public TurretControlPhysics(
+      Translation2d turretOffsetRobotFrame,
+      Rotation2d minTurretAngle,
+      Rotation2d maxTurretAngle,
+      Rotation2d feedforwardPaddingAngle,
+      double settlingGain,
+      DoubleFunction<Double> timeOfFlightFunc,
+      BiFunction<Double, Double, Double> settlingTimeFunc,
+      double minRangeMeters,
+      double maxRangeMeters) {
+    this.turretOffsetRobotFrame = turretOffsetRobotFrame;
+    SmartDashboard.putNumber("Turret Offset X", turretOffsetRobotFrame.getX());
+    SmartDashboard.putNumber("Turret Offset Y", turretOffsetRobotFrame.getY());
+    SmartDashboard.putNumber("Turret Offset Angle", turretOffsetRobotFrame.getAngle().getDegrees());
+
+    this.minTurretAngle = minTurretAngle;
+    this.maxTurretAngle = maxTurretAngle;
+    this.feedforwardPaddingAngle = feedforwardPaddingAngle;
+    this.settlingTimeGain = settlingGain;
+    this.timeOfFlightFunction = timeOfFlightFunc;
+    this.settlingTimeFunction = settlingTimeFunc;
+    this.minEffectiveRangeMeters = minRangeMeters;
+    this.maxEffectiveRangeMeters = maxRangeMeters;
+  }
+  /** Defines possible states for the aiming status */
+  public enum AimingStatus {
+    READY_TO_FIRE,
+    TARGET_TOO_CLOSE,
+    TARGET_TOO_FAR,
+    IN_DEADZONE,
+    SOLVER_FAILED
+  }
+  /** Defines data representing for the solver result */
+  public record AimingSolution(
+      Translation2d virtualTargetFieldPos,
+      Rotation2d turretFieldHeading,
+      Rotation2d turretLocalHeading,
+      double turretFeedforwardRadPerSec,
+      double effectiveDistanceMeters,
+      double estimatedTimeOfFlight,
+      AimingStatus status,
+      SolverState finalSolverState) {
+    public boolean isPossible() {
+      return status == AimingStatus.READY_TO_FIRE;
+    }
+  }
+
+  /**
+   * Solves for the optimal turret angle and feedforward velocity.
+   *
+   * @param targetFieldPos The field-relative position of the target.
+   * @param currentTurretAngle The current robot-relative angle of the turret.
+   * @param currentTurretVelocityRadPerSec The current turret angular velocity in rad/s.
+   * @param predictor The prediction logic to estimate future robot states.
+   * @return A complete aiming solution including setpoints and status.
+   */
+  public AimingSolution solve(
+      Translation2d targetFieldPos,
+      Rotation2d currentTurretAngle,
+      double currentTurretVelocityRadPerSec,
+      RobotPredictor predictor) {
+
+    SolverState finalState =
+        runNewtonSolver(
+            targetFieldPos, currentTurretAngle, currentTurretVelocityRadPerSec, predictor);
+
+    Rotation2d fieldHeading = getAngleFromVector(finalState.vectorToVirtualTarget);
+    Rotation2d localHeading = fieldHeading.minus(finalState.robotStateAtFire.pose().getRotation());
+
+    double feedforwardRadPerSec = calculateKinematicFeedforward(finalState);
+
+    AimingStatus status = AimingStatus.READY_TO_FIRE;
+    double distanceToTarget = finalState.vectorToVirtualTarget.getNorm();
+    /** Checks the distance to the target for effective shooting range */
+    if (distanceToTarget < minEffectiveRangeMeters) {
+      status = AimingStatus.TARGET_TOO_CLOSE;
+    } else if (distanceToTarget > maxEffectiveRangeMeters) {
+      status = AimingStatus.TARGET_TOO_FAR;
+    } else if (!finalState.hasConverged) {
+      status = AimingStatus.SOLVER_FAILED;
+    }
+
+    double localHeadingRadians = MathUtil.angleModulus(localHeading.getRadians());
+    double minLimitRadians = minTurretAngle.getRadians();
+    double maxLimitRadians = maxTurretAngle.getRadians();
+
+    if (localHeadingRadians < minLimitRadians || localHeadingRadians > maxLimitRadians) {
+      status = AimingStatus.IN_DEADZONE;
+      /**
+       * If the angle is outside limits, then it clamps to a valid limit based on motion direction
+       */
+      if (feedforwardRadPerSec > 0.1) {
+        localHeading = minTurretAngle;
+      } else if (feedforwardRadPerSec < -0.1) {
+        localHeading = maxTurretAngle;
+      } else {
+        /** Sets the turret closest angle limit if the feed forward is near zero */
+        double distanceToMin =
+            Math.abs(MathUtil.angleModulus(localHeadingRadians - minLimitRadians));
+        double distanceToMax =
+            Math.abs(MathUtil.angleModulus(localHeadingRadians - maxLimitRadians));
+        localHeading = (distanceToMin < distanceToMax) ? minTurretAngle : maxTurretAngle;
+      }
+
+      feedforwardRadPerSec = 0.0;
+
+    } else {
+      feedforwardRadPerSec =
+          applyFeedforwardSafetyPadding(localHeadingRadians, feedforwardRadPerSec);
+    }
+
+    return new AimingSolution(
+        finalState.virtualTargetFieldPos,
+        fieldHeading,
+        localHeading,
+        feedforwardRadPerSec,
+        distanceToTarget,
+        finalState.requiredTimeOfFlight,
+        status,
+        finalState);
+  }
+  /** Scales feedforward when the turret is near mechanical limits */
+  private double applyFeedforwardSafetyPadding(
+      double currentAngleRadians, double commandedFeedforward) {
+    double minLimitRadians = minTurretAngle.getRadians();
+    double maxLimitRadians = maxTurretAngle.getRadians();
+    double paddingRadians = feedforwardPaddingAngle.getRadians();
+
+    if (commandedFeedforward > 0 && currentAngleRadians > (maxLimitRadians - paddingRadians)) {
+      double distanceToLimit = maxLimitRadians - currentAngleRadians;
+      double scaleFactor = MathUtil.clamp(distanceToLimit / paddingRadians, 0.0, 1.0);
+      return commandedFeedforward * scaleFactor;
+    }
+
+    if (commandedFeedforward < 0 && currentAngleRadians < (minLimitRadians + paddingRadians)) {
+      double distanceToLimit = currentAngleRadians - minLimitRadians;
+      double scaleFactor = MathUtil.clamp(distanceToLimit / paddingRadians, 0.0, 1.0);
+      return commandedFeedforward * scaleFactor;
+    }
+
+    return commandedFeedforward;
+  }
+  /**
+   * Runs the Newton solver to converge the right time of flight so the launcher can shoot when
+   * moving
+   */
+  private SolverState runNewtonSolver(
+      Translation2d targetFieldPos,
+      Rotation2d currentTurretAngle,
+      double currentTurretVelocityRadPerSec,
+      RobotPredictor predictor) {
+
+    double timeFlightGuess = 0.5;
+    SolverState bestState = null;
+    /** Computes the solver state for the current guess for the current time of flight guess */
+    for (int i = 0; i < MAX_SOLVER_ITERATIONS; i++) {
+      SolverState stateCurrent =
+          computePhysicsState(
+              timeFlightGuess,
+              targetFieldPos,
+              currentTurretAngle,
+              currentTurretVelocityRadPerSec,
+              predictor);
+
+      if (Math.abs(stateCurrent.errorSeconds) < CONVERGENCE_THRESHOLD_SECONDS) {
+        return stateCurrent.markConverged();
+      }
+      /** Computes the solver state for the derivative probe */
+      SolverState stateProbe =
+          computePhysicsState(
+              timeFlightGuess + DERIVATIVE_PROBE_TIME_DELTA,
+              targetFieldPos,
+              currentTurretAngle,
+              currentTurretVelocityRadPerSec,
+              predictor);
+
+      double slope =
+          (stateProbe.errorSeconds - stateCurrent.errorSeconds) / DERIVATIVE_PROBE_TIME_DELTA;
+
+      if (Math.abs(slope) < 1e-5) slope = Math.signum(slope) * 1e-5;
+
+      double newGuess = timeFlightGuess - (stateCurrent.errorSeconds / slope);
+      timeFlightGuess = Math.max(0.01, newGuess);
+
+      bestState = stateCurrent;
+    }
+    return bestState;
+  }
+  /** Computes and returns a solver state */
+  private SolverState computePhysicsState(
+      double timeFlightGuess,
+      Translation2d targetFieldPos,
+      Rotation2d currentTurretAngle,
+      double currentTurretVelocityRadPerSec,
+      RobotPredictor predictor) {
+
+    RobotState stateNow = predictor.predict(0.0, 0.0);
+
+    Translation2d estimatedVirtualTarget =
+        targetFieldPos.minus(
+            stateNow.velocity() != null
+                ? new Translation2d(
+                        stateNow.velocity().vxMetersPerSecond,
+                        stateNow.velocity().vyMetersPerSecond)
+                    .times(timeFlightGuess)
+                : new Translation2d());
+    /**
+     * Computes the robot heading, turret offset, and vector from the turret to the estimated target
+     */
+    Rotation2d robotHeadingNow = stateNow.pose().getRotation();
+    Translation2d turretOffsetNow = turretOffsetRobotFrame.rotateBy(robotHeadingNow);
+    Translation2d vectorToEstimatedTarget =
+        estimatedVirtualTarget.minus(stateNow.pose().getTranslation().plus(turretOffsetNow));
+    /** Computes the angle to the estimated target */
+    Rotation2d goalAngleLocal = getAngleFromVector(vectorToEstimatedTarget).minus(robotHeadingNow);
+    double angleErrorRadians =
+        Math.abs(MathUtil.angleModulus(goalAngleLocal.minus(currentTurretAngle).getRadians()));
+
+    double estimatedSettlingTime =
+        settlingTimeFunction.apply(angleErrorRadians, currentTurretVelocityRadPerSec)
+            * settlingTimeGain;
+
+    // Cap prediction lookahead to prevent heading over-extrapolation during fast rotation.
+    // Linear heading prediction (heading += omega * dt) becomes unreliable beyond ~0.15s
+    // and causes the solved turret goal to jump discontinuously when robot angular velocity
+    // changes.
+    double predictedSettlingTime = Math.min(estimatedSettlingTime, 0.15);
+
+    RobotState stateAtFire = predictor.predict(0.0, predictedSettlingTime);
+    Rotation2d headingAtFire = stateAtFire.pose().getRotation();
+    Translation2d turretOffsetAtFire = turretOffsetRobotFrame.rotateBy(headingAtFire);
+
+    double robotAngularVelocity = stateAtFire.velocity().omegaRadiansPerSecond;
+
+    Translation2d tangentialVelocity =
+        new Translation2d(
+            -robotAngularVelocity * turretOffsetAtFire.getY(),
+            robotAngularVelocity * turretOffsetAtFire.getX());
+
+    Translation2d robotLinearVelocity =
+        new Translation2d(
+            stateAtFire.velocity().vxMetersPerSecond, stateAtFire.velocity().vyMetersPerSecond);
+
+    Translation2d inheritedMuzzleVelocity = robotLinearVelocity.plus(tangentialVelocity);
+
+    Translation2d virtualTargetPos =
+        targetFieldPos.minus(inheritedMuzzleVelocity.times(timeFlightGuess));
+
+    Translation2d gunPositionAtFire = stateAtFire.pose().getTranslation().plus(turretOffsetAtFire);
+    Translation2d vectorToVirtualTarget = virtualTargetPos.minus(gunPositionAtFire);
+
+    double distanceToVirtualTarget = vectorToVirtualTarget.getNorm();
+    double requiredTimeOfFlight = timeOfFlightFunction.apply(distanceToVirtualTarget);
+
+    double errorSeconds = timeFlightGuess - requiredTimeOfFlight;
+    /** Returns a fully constructed solver state */
+    return new SolverState(
+        errorSeconds,
+        requiredTimeOfFlight,
+        virtualTargetPos,
+        vectorToVirtualTarget,
+        inheritedMuzzleVelocity,
+        stateAtFire,
+        false);
+  }
+  /** Computes the kinematic feed forward caused by robot rotation and acceleration */
+  private double calculateKinematicFeedforward(SolverState state) {
+    RobotState robotState = state.robotStateAtFire;
+    Rotation2d robotHeading = robotState.pose().getRotation();
+    Translation2d turretOffsetRotated = turretOffsetRobotFrame.rotateBy(robotHeading);
+
+    double robotOmega = robotState.velocity().omegaRadiansPerSecond;
+    double robotAlpha =
+        robotState.acceleration() != null ? robotState.acceleration().omegaRadiansPerSecond : 0.0;
+
+    Translation2d accelTangential =
+        new Translation2d(
+            -robotAlpha * turretOffsetRotated.getY(), robotAlpha * turretOffsetRotated.getX());
+
+    Translation2d accelCentripetal = turretOffsetRotated.times(-(robotOmega * robotOmega));
+    /**
+     * Converts the robot's linear acceleration into a 2D vector and defaults it to 0 if no data is
+     * available
+     */
+    Translation2d accelRobotLinear =
+        robotState.acceleration() != null
+            ? new Translation2d(
+                robotState.acceleration().vxMetersPerSecond,
+                robotState.acceleration().vyMetersPerSecond)
+            : new Translation2d();
+
+    Translation2d accelTurretMount = accelRobotLinear.plus(accelTangential).plus(accelCentripetal);
+
+    Translation2d velocityVirtualTargetDrift = accelTurretMount.times(-state.requiredTimeOfFlight);
+
+    Translation2d velocityRelative =
+        velocityVirtualTargetDrift.minus(state.inheritedMuzzleVelocity);
+
+    double distanceSquared = Math.pow(state.vectorToVirtualTarget.getNorm(), 2);
+
+    if (distanceSquared < 1e-4) return 0.0;
+
+    double crossProduct =
+        (state.vectorToVirtualTarget.getX() * velocityRelative.getY())
+            - (state.vectorToVirtualTarget.getY() * velocityRelative.getX());
+
+    double omegaFieldRelative = crossProduct / distanceSquared;
+
+    return omegaFieldRelative - robotOmega;
+  }
+
+  private Rotation2d getAngleFromVector(Translation2d vec) {
+    return new Rotation2d(vec.getX(), vec.getY());
+  }
+  /** Packages physics data from the newton solver into one immutable object */
+  public record SolverState(
+      double errorSeconds,
+      double requiredTimeOfFlight,
+      Translation2d virtualTargetFieldPos,
+      Translation2d vectorToVirtualTarget,
+      Translation2d inheritedMuzzleVelocity,
+      RobotState robotStateAtFire,
+      boolean hasConverged) {
+    /** Returns the new solver state that has converged */
+    public SolverState markConverged() {
+      return new SolverState(
+          errorSeconds,
+          requiredTimeOfFlight,
+          virtualTargetFieldPos,
+          vectorToVirtualTarget,
+          inheritedMuzzleVelocity,
+          robotStateAtFire,
+          true);
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/intake/Intake.java b/src/main/java/frc/robot/rebuilt/subsystems/intake/Intake.java
new file mode 100644
index 00000000..1595afb5
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/intake/Intake.java
@@ -0,0 +1,146 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.rebuilt.subsystems.intake;
+
+import static edu.wpi.first.units.Units.Degrees;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.rebuilt.commands.IntakeCommands;
+import frc.robot.rebuilt.commands.IntakeCommands.IntakeState;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.sensors.Controller;
+import org.littletonrobotics.junction.Logger;
+
+public class Intake extends GenericSubsystem {
+  private IntakeIO io;
+  private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+
+  /** Creates a new Intake and selects the IO */
+  public Intake() {
+    super("intake.json");
+    if (RobotBase.isSimulation()) {
+      io = new IntakeIOSim(devices);
+    } else {
+      io = new IntakeIOReal(devices);
+    }
+  }
+
+  public void runSpintake(double speed) {
+    io.runSpintake(speed);
+  }
+
+  public void runSpintakes(double outerSpeed, double innerSpeed) {
+    io.runSpintakes(innerSpeed, outerSpeed);
+  }
+
+  /** Creates a command that runs the spintake at the given speed and stops when done */
+  public Command spintakeCommand(double speed) {
+    return Commands.run(
+            () -> {
+              runSpintake(speed);
+            })
+        .finallyDo(
+            () -> {
+              runSpintake(0);
+            });
+  }
+
+  public Command setDesiredHopperAngle(Angle angle) {
+    return io.setHopperAngle(angle);
+  }
+
+  public Angle getHopperAngle() {
+    return inputs.hopperAngleActual;
+  }
+
+  public boolean isRetracted() {
+    return io.isRetracted();
+  }
+
+  public boolean isDeployed() {
+    return io.isDeployed();
+  }
+
+  public void runHopper(double speed) {
+    io.runHopper(speed);
+  }
+  /** Configures test controller bindings for the spintake, hopper control, and sysid */
+  public void configTestController(Controller controller) {
+    controller.createRightBumper().whileTrue(spintakeCommand(0.5));
+    controller.createYButton().whileTrue(getHopperSysIdCommand());
+    controller.setRightYAxis(controller.createRightYAxis());
+    Trigger rightYAxis = new Trigger(() -> controller.getRightYAxis() > 0.01);
+    rightYAxis.whileTrue(Commands.run(() -> runHopper(controller.getRightYAxis())));
+  }
+  /** Updates intake inputs from the io periodically and logs them each robot cycle */
+  @Override
+  public void periodic() {
+    super.periodic();
+    io.updateInputs(inputs);
+    Logger.processInputs("Intake", inputs);
+  }
+
+  public boolean isRequested(IntakeState state) {
+    return inputs.stateRequested == state;
+  }
+
+  public boolean isCurrent(IntakeState state) {
+    return inputs.stateCurrent == state;
+  }
+
+  public boolean isNearTrench() {
+    return isCurrent(IntakeState.DEPLOYING) && io.isNearTrench();
+  }
+
+  public void setCurrentState(IntakeState state) {
+    inputs.stateCurrent = state;
+  }
+
+  public IntakeState getCurrentState() {
+    return inputs.stateCurrent;
+  }
+
+  public boolean isHopperStalling() {
+    return io.isHopperStalling();
+  }
+
+  public Command getHopperSysIdCommand() {
+    return io.getHopperSysIdCommand();
+  }
+
+  public Command getHopperCharacterizationCommand() {
+    return io.getHopperCharacterizationCommand(this);
+  }
+
+  public void setRequestedState(IntakeState state) {
+    inputs.stateRequested = state;
+  }
+
+  public void setHopperDeployed() {
+    io.setHopperPosition(Degrees.of(0));
+    setRequestedState(IntakeCommands.IntakeState.DEPLOYED);
+  }
+
+  public void setHopperRetracted() {
+    io.setHopperPosition(Degrees.of(120));
+    setRequestedState(IntakeCommands.IntakeState.RETRACTED);
+  }
+
+  public boolean isHopperMoving() {
+    return io.isHopperMoving();
+  }
+
+  public void setHopperPosition(Angle angle) {
+    io.setHopperPosition(angle);
+  }
+
+  public boolean isHopperAtGoal() {
+    return inputs.hopperAtGoal;
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIO.java
new file mode 100644
index 00000000..d9aee0c1
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIO.java
@@ -0,0 +1,52 @@
+package frc.robot.rebuilt.subsystems.intake;
+
+import static edu.wpi.first.units.Units.Degrees;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.rebuilt.commands.IntakeCommands;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+  @AutoLog
+  public static class IntakeIOInputs {
+    public IntakeCommands.IntakeState stateRequested = IntakeCommands.IntakeState.UNKNOWN;
+    public IntakeCommands.IntakeState stateCurrent = IntakeCommands.IntakeState.UNKNOWN;
+    public double speed = 0.0;
+    public Angle hopperAngleActual = Degrees.of(0.0);
+    public double hopperAngleDegrees = 0.0;
+    public double hopperAmps = 0;
+
+    public Angle hopperAngleDesired = Degrees.of(0);
+    public double hopperAngleError = 0.0;
+    public boolean hopperAtGoal = true;
+    public int simulatedGamepieces = 0;
+  }
+
+  public void runSpintake(double speed);
+
+  public void runSpintakes(double outerSpeed, double innerSpeed);
+
+  public Command setHopperAngle(Angle angle);
+
+  public void setHopperPosition(Angle angle);
+
+  public boolean isHopperMoving();
+
+  public boolean isRetracted();
+
+  public boolean isDeployed();
+
+  public boolean isHopperStalling();
+
+  public void runHopper(double speed);
+
+  public boolean isNearTrench();
+
+  public Command getHopperSysIdCommand();
+
+  public Command getHopperCharacterizationCommand(GenericSubsystem intake);
+
+  public default void updateInputs(IntakeIOInputs inputs) {}
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOReal.java
new file mode 100644
index 00000000..be9847da
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOReal.java
@@ -0,0 +1,182 @@
+package frc.robot.rebuilt.subsystems.intake;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.rebuilt.Constants;
+import frc.robot.rebuilt.FieldConstants;
+import frc.robot.rebuilt.commands.IntakeCommands;
+import java.util.Map;
+import org.frc5010.common.arch.GenericSubsystem;
+import org.frc5010.common.drive.GenericDrivetrain;
+import org.frc5010.common.motors.SystemIdentification;
+import org.littletonrobotics.junction.Logger;
+import yams.mechanisms.positional.Arm;
+import yams.mechanisms.velocity.FlyWheel;
+
+public class IntakeIOReal implements IntakeIO {
+  protected Map<String, Object> devices;
+  private FlyWheel spintakeInner;
+  private FlyWheel spintakeOuter;
+  private Arm intakeHopper;
+  protected GenericDrivetrain drivetrain;
+  private boolean isNearTrench = false;
+  private IntakeCommands.IntakeState lastState = IntakeCommands.IntakeState.RETRACTED;
+
+  /** initializes the spintake and hopper */
+  public IntakeIOReal(Map<String, Object> devices) {
+    this.devices = devices;
+    // spintakeLead = (FlyWheel) devices.get("spintake");
+    spintakeOuter = (FlyWheel) devices.get("spintake_outer");
+    spintakeInner = (FlyWheel) devices.get("spintake_inner");
+    intakeHopper = (Arm) devices.get("hopper");
+  }
+
+  @Override
+  public void runSpintake(double speed) {
+    spintakeOuter.getMotor().setDutyCycle(speed);
+    spintakeInner.getMotor().setDutyCycle(Constants.Intake.INTAKE_INNER_IN);
+  }
+
+  public void runSpintakes(double outerSpeed, double innerSpeed) {
+    spintakeOuter.getMotor().setDutyCycle(outerSpeed);
+    spintakeInner.getMotor().setDutyCycle(innerSpeed);
+  }
+
+  public Command setHopperAngle(Angle angle) {
+    return intakeHopper.setAngle(angle);
+  }
+
+  public void setHopperPosition(Angle angle) {
+    intakeHopper.getMotor().setEncoderPosition(angle);
+  }
+
+  public boolean isHopperMoving() {
+    return Math.abs(
+            intakeHopper.getMotorController().getMechanismVelocity().in(Degrees.per(Second)))
+        > 1.0;
+  }
+
+  public boolean isHopperStalling() {
+    return Math.abs(intakeHopper.getMotor().getStatorCurrent().in(Amps))
+        > Constants.Intake.HOPPER_STALL_CURRENT_THRESHOLD;
+  }
+
+  public boolean isRetracted() {
+    return (intakeHopper.getAngle().gte(Constants.Intake.HOPPER_RETRACTED_ANGLE));
+  }
+
+  public boolean isDeployed() {
+    return (intakeHopper.getAngle().lte(Degrees.of(2.0)));
+  }
+
+  public Command getHopperSysIdCommand() {
+    return intakeHopper.sysId(Volts.of(4), Volts.of(0.5).per(Seconds), Seconds.of(8));
+  }
+  /** Returns a sysid command for the hopper */
+  public Command getHopperSysIdCommand(GenericSubsystem intake) {
+    return SystemIdentification.getSysIdFullCommand(
+        SystemIdentification.angleSysIdRoutine(
+            intakeHopper.getMotorController(), intakeHopper.getName(), intake),
+        5,
+        5,
+        3,
+        () ->
+            intakeHopper
+                .isNear(
+                    intakeHopper.getMotorController().getConfig().getMechanismUpperLimit().get(),
+                    Degrees.of(10))
+                .getAsBoolean(),
+        () ->
+            intakeHopper
+                .isNear(
+                    intakeHopper.getMotorController().getConfig().getMechanismLowerLimit().get(),
+                    Degrees.of(10))
+                .getAsBoolean(),
+        () -> intakeHopper.getMotor().setDutyCycle(0));
+  }
+
+  public Command getHopperCharacterizationCommand(GenericSubsystem intake) {
+    return SystemIdentification.feedforwardCharacterization(
+        intake,
+        (Voltage voltage) -> intakeHopper.getMotor().setVoltage(voltage),
+        () -> intakeHopper.getMotorController().getMechanismVelocity().in(Degrees.per(Second)));
+  }
+
+  public void runHopper(double speed) {
+    intakeHopper.getMotorController().setDutyCycle(speed);
+  }
+
+  public boolean isNearTrench() {
+    Pose2d current = drivetrain.getPoseEstimator().getCurrentPose();
+    double currentX = current.getX();
+    double currentY = current.getY();
+
+    double topTrenchLeftX = FieldConstants.TrenchZoneTop.nearAllianceLeftDanger.getX();
+    double topTrenchRightX = FieldConstants.TrenchZoneTop.nearAllianceRightDanger.getX();
+
+    double topTrenchY = FieldConstants.TrenchZoneTop.nearAllianceLeftDanger.getY();
+
+    double topOppTrenchLeftX = FieldConstants.TrenchZoneTop.oppAllianceLeftDanger.getX();
+    double topOppTrenchRightX = FieldConstants.TrenchZoneTop.oppAllianceRightDanger.getX();
+
+    double lowerTrenchLeftX = FieldConstants.TrenchZoneBottom.nearAllianceLeftDanger.getX();
+    double lowerTrenchRightX = FieldConstants.TrenchZoneBottom.nearAllianceRightDanger.getX();
+
+    double lowerTrenchY = FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger.getY();
+
+    double lowerOppTrenchLeftX = FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger.getX();
+    double lowerOppTrenchRightX = FieldConstants.TrenchZoneBottom.oppAllianceRightDanger.getX();
+
+    boolean nearAllianceTop =
+        ((currentX > topTrenchLeftX && currentX < topTrenchRightX) && currentY > topTrenchY);
+
+    boolean nearOppAllianceTop =
+        ((currentX > topOppTrenchLeftX && currentX < topOppTrenchRightX) && currentY > topTrenchY);
+
+    boolean nearAllianceBottom =
+        ((currentX > lowerTrenchLeftX && currentX < lowerTrenchRightX) && currentY < lowerTrenchY);
+
+    boolean nearOppAllianceBottom =
+        ((currentX > lowerOppTrenchLeftX && currentX < lowerOppTrenchRightX)
+            && currentY < lowerTrenchY);
+
+    SmartDashboard.putBoolean("Near Top Opp Alliance", nearOppAllianceTop);
+    SmartDashboard.putBoolean("Near Top Alliance", nearAllianceTop);
+    SmartDashboard.putBoolean("Near Bottom Opp Alliance", nearOppAllianceBottom);
+    SmartDashboard.putBoolean("Near Bottom Alliance", nearAllianceBottom);
+
+    if (nearAllianceTop || nearOppAllianceTop || nearAllianceBottom || nearOppAllianceBottom)
+      return true;
+    else {
+      return false;
+    }
+  }
+
+  /** updates the input structure with the current hopper and intake speed */
+  @Override
+  public void updateInputs(IntakeIOInputs inputs) {
+    Logger.recordOutput(
+        "Hopper Velocity",
+        intakeHopper.getMotorController().getMechanismVelocity().in(Degrees.per(Second)));
+    Logger.recordOutput("Hopper MOving", isHopperMoving());
+    inputs.hopperAngleActual = intakeHopper.getMotorController().getMechanismPosition();
+    inputs.hopperAngleDegrees = inputs.hopperAngleActual.in(Degrees);
+    inputs.hopperAngleDesired =
+        intakeHopper.getMotorController().getMechanismPositionSetpoint().orElse(Degrees.of(0));
+    inputs.hopperAngleError = inputs.hopperAngleDesired.minus(inputs.hopperAngleActual).in(Degrees);
+    inputs.hopperAtGoal =
+        Math.abs(inputs.hopperAngleError) < Constants.Intake.HOPPER_ANGLE_TOLERANCE;
+    inputs.speed = spintakeOuter.getMotor().getDutyCycle();
+    inputs.hopperAmps = intakeHopper.getMotor().getStatorCurrent().in(Amps);
+    // inputs.speed = spintakeLead.getMotor().getDutyCycle();
+  }
+}
diff --git a/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOSim.java
new file mode 100644
index 00000000..dd79c731
--- /dev/null
+++ b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOSim.java
@@ -0,0 +1,56 @@
+package frc.robot.rebuilt.subsystems.intake;
+
+import static edu.wpi.first.units.Units.Inches;
+
+import frc.robot.rebuilt.Rebuilt;
+import java.util.Map;
+import org.frc5010.common.drive.GenericDrivetrain;
+import swervelib.simulation.ironmaple.simulation.IntakeSimulation;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
+import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation;
+import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnField;
+
+/** Simulates the implimentation of IntakeIO */
+public class IntakeIOSim extends IntakeIOReal {
+  public static IntakeSimulation intakeSimulation;
+  private AbstractDriveTrainSimulation driveTrainSimulation;
+  private GamePieceOnFieldSimulation gamePiece;
+  /** Initializes the mapleSim intake simulation */
+  public IntakeIOSim(Map<String, Object> devices) {
+    super(devices);
+    driveTrainSimulation = GenericDrivetrain.getMapleSimDrive().get();
+    intakeSimulation =
+        IntakeSimulation.OverTheBumperIntake(
+            "Fuel",
+            driveTrainSimulation,
+            Inches.of(27.25),
+            Inches.of(11.25),
+            IntakeSimulation.IntakeSide.FRONT,
+            80);
+  }
+  /** Runs the intake motor and updates the state of the intake simulation */
+  @Override
+  public void runSpintake(double speed) {
+    super.runSpintake(speed);
+    if (speed > 0) {
+      intakeSimulation.startIntake();
+    } else {
+      intakeSimulation.stopIntake();
+    }
+  }
+  /** manages simulated collection of game pieces and updates intake inputs */
+  @Override
+  public void updateInputs(IntakeIOInputs inputs) {
+    super.updateInputs(inputs);
+    if (inputs.speed < 0) {
+      if (intakeSimulation.obtainGamePieceFromIntake()) {
+        gamePiece =
+            new RebuiltFuelOnField(
+                Rebuilt.drivetrain.getPoseEstimator().getCurrentPose().getTranslation());
+        SimulatedArena.getInstance().addGamePiece(gamePiece);
+      }
+    }
+    inputs.simulatedGamepieces = intakeSimulation.getGamePiecesAmount();
+  }
+}
diff --git a/src/main/java/org/frc5010/common/arch/GenericSubsystem.java b/src/main/java/org/frc5010/common/arch/GenericSubsystem.java
index 95a44359..d819021e 100644
--- a/src/main/java/org/frc5010/common/arch/GenericSubsystem.java
+++ b/src/main/java/org/frc5010/common/arch/GenericSubsystem.java
@@ -4,6 +4,7 @@
 
 package org.frc5010.common.arch;
 
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
@@ -124,16 +125,14 @@ public void initSendable(SendableBuilder builder) {
   @Override
   public void periodic() {
     DashBoard.notifyListeners();
-    devices.values().stream()
-        .forEach(
-            it -> {
-              if (it instanceof GenericFunctionalMotor) {
-                ((GenericFunctionalMotor) it).periodicUpdate();
-              }
-              if (it instanceof SmartMechanism) {
-                ((SmartMechanism) it).updateTelemetry();
-              }
-            });
+    for (Object it : devices.values()) {
+      if (it instanceof GenericFunctionalMotor) {
+        ((GenericFunctionalMotor) it).periodicUpdate();
+      }
+      if (it instanceof SmartMechanism) {
+        ((SmartMechanism) it).updateTelemetry();
+      }
+    }
   }
 
   /**
@@ -142,16 +141,14 @@ public void periodic() {
    */
   @Override
   public void simulationPeriodic() {
-    devices.values().stream()
-        .forEach(
-            it -> {
-              if (it instanceof GenericFunctionalMotor) {
-                ((GenericFunctionalMotor) it).simulationUpdate();
-              }
-              if (it instanceof SmartMechanism) {
-                ((SmartMechanism) it).simIterate();
-              }
-            });
+    for (Object it : devices.values()) {
+      if (it instanceof GenericFunctionalMotor) {
+        ((GenericFunctionalMotor) it).simulationUpdate();
+      }
+      if (it instanceof SmartMechanism) {
+        ((SmartMechanism) it).simIterate();
+      }
+    }
   }
 
   /**
@@ -172,4 +169,9 @@ public DisplayValuesHelper getDisplayValuesHelper() {
   public void setDisplay(boolean display) {
     if (display) DashBoard.makeDisplayed();
   }
+
+  public static Object setHeight(Distance of) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setHeight'");
+  }
 }
diff --git a/src/main/java/org/frc5010/common/commands/AkitDriveCommands.java b/src/main/java/org/frc5010/common/commands/AkitDriveCommands.java
index f4a75091..5f9b4029 100644
--- a/src/main/java/org/frc5010/common/commands/AkitDriveCommands.java
+++ b/src/main/java/org/frc5010/common/commands/AkitDriveCommands.java
@@ -399,8 +399,7 @@ public static Command drivePIDTuning(GenericSwerveDrivetrain swerveDrive, AkitSw
                                       double avgVelocity = 0.0;
                                       for (int i = 0; i < 4; i++) {
                                         avgVelocity +=
-                                            drive.getModulesInfo()[i]
-                                                .driveVelocityMetersPerSecond();
+                                            drive.getModulesInfo()[i].driveVelocityMetersPerSecond;
                                       }
                                       avgVelocity /= 4.0;
                                       double error = Math.abs(targetVelocity - avgVelocity);
@@ -497,7 +496,7 @@ public static Command steerPIDTuning(GenericSwerveDrivetrain swerveDrive, AkitSw
                                   double avgAngleError = 0.0;
                                   for (int i = 0; i < 4; i++) {
                                     double currentAngle =
-                                        drive.getModulesInfo()[i].steerAbsoluteDegrees();
+                                        drive.getModulesInfo()[i].steerAbsoluteDegrees;
                                     double error = Math.abs(targetDegrees - currentAngle);
                                     // Handle angle wrapping (shortest path)
                                     if (error > 180.0) {
diff --git a/src/main/java/org/frc5010/common/config/RobotParser.java b/src/main/java/org/frc5010/common/config/RobotParser.java
index 6d478bf3..2d8bc04d 100644
--- a/src/main/java/org/frc5010/common/config/RobotParser.java
+++ b/src/main/java/org/frc5010/common/config/RobotParser.java
@@ -19,6 +19,7 @@
 import org.frc5010.common.config.json.VisionPropertiesJson;
 import org.frc5010.common.config.json.YAGSLDrivetrainJson;
 import org.frc5010.common.config.json.devices.LEDStripParser;
+import org.frc5010.common.config.json.devices.OrchestraParser;
 
 /**
  * Parses JSON configuration files to initialize and build a robot's subsystems.
@@ -108,6 +109,9 @@ public RobotParser(String robotDirectory, GenericRobot robot) throws IOException
     // Parse LED strips
     LEDStripParser.parse(robotDirectory);
 
+    // Parse the orchestra configuration (if it exists)
+    OrchestraParser.parse(robotDirectory);
+
     // Read in the drivetrain
     switch (robotJson.driveType) {
       case "YAGSL_SWERVE_DRIVE":
diff --git a/src/main/java/org/frc5010/common/config/json/devices/OrchestraConfigJson.java b/src/main/java/org/frc5010/common/config/json/devices/OrchestraConfigJson.java
new file mode 100644
index 00000000..8b49e104
--- /dev/null
+++ b/src/main/java/org/frc5010/common/config/json/devices/OrchestraConfigJson.java
@@ -0,0 +1,21 @@
+package org.frc5010.common.config.json.devices;
+
+import org.frc5010.common.utils.OrchestraManager;
+
+public class OrchestraConfigJson {
+  public int[] rioIds = {};
+
+  // TalonFX CAN IDs on the CANivore bus
+  public int[] canivoreIds = {};
+
+  public static class MusicEntry {
+    public String name = "";
+    public String path = "";
+  }
+
+  public MusicEntry[] music = {};
+
+  public void configure() {
+    OrchestraManager.init(this);
+  }
+}
diff --git a/src/main/java/org/frc5010/common/config/json/devices/OrchestraParser.java b/src/main/java/org/frc5010/common/config/json/devices/OrchestraParser.java
new file mode 100644
index 00000000..bc6a1f55
--- /dev/null
+++ b/src/main/java/org/frc5010/common/config/json/devices/OrchestraParser.java
@@ -0,0 +1,25 @@
+package org.frc5010.common.config.json.devices;
+
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.wpilibj.Filesystem;
+import java.io.File;
+import java.io.IOException;
+
+public class OrchestraParser {
+  public static void parse(String robotDirectory) {
+    try {
+      File directory = new File(Filesystem.getDeployDirectory(), robotDirectory + "/subsystems");
+      DeviceConfigReader.checkDirectory(directory);
+      File deviceFile = new File(directory, "orchestra.json");
+      if (!deviceFile.exists()) {
+        return;
+      }
+      OrchestraConfigJson orchestraConfig =
+          new ObjectMapper().readValue(deviceFile, OrchestraConfigJson.class);
+      orchestraConfig.configure();
+    } catch (IOException e) {
+      System.out.println("Error reading device configuration: " + e.getMessage());
+      return;
+    }
+  }
+}
diff --git a/src/main/java/org/frc5010/common/drive/pose/DrivePoseEstimator.java b/src/main/java/org/frc5010/common/drive/pose/DrivePoseEstimator.java
index 23e54c42..0a796a28 100644
--- a/src/main/java/org/frc5010/common/drive/pose/DrivePoseEstimator.java
+++ b/src/main/java/org/frc5010/common/drive/pose/DrivePoseEstimator.java
@@ -24,7 +24,6 @@
 import java.util.ArrayList;
 import java.util.List;
 import java.util.function.Function;
-import java.util.stream.Collectors;
 import org.frc5010.common.arch.GenericSubsystem;
 import org.frc5010.common.commands.calibration.PoseProviderAutoOffset;
 import org.frc5010.common.drive.GenericDrivetrain;
@@ -47,6 +46,12 @@ public class DrivePoseEstimator extends GenericSubsystem {
   private boolean disableVisionUpdateCommand = false;
   /** List of PoseProviders */
   private List<PoseProvider> poseProviders = new ArrayList<>();
+  /** Reusable filtered list to avoid per-cycle stream/collect allocation */
+  private final List<PoseProvider> activePoseProviders = new ArrayList<>();
+  /** Cached current pose in 3D — updated once per periodic() to avoid per-call allocation */
+  private Pose3d cachedPose3d = new Pose3d();
+  /** Pre-allocated array for Shuffleboard pose3d widget — filled in-place each cycle */
+  private final double[] pose3dArray = new double[7];
 
   private DisplayBoolean aprilTagVisible = DashBoard.makeDisplayBoolean("AprilTagVisible");
   private boolean updatingPoseAcceptor = false;
@@ -188,7 +193,7 @@ public Pose2d getCurrentPose() {
    * @return the current pose with z = 0
    */
   public Pose3d getCurrentPose3d() {
-    return new Pose3d(poseTracker.getCurrentPose());
+    return cachedPose3d;
   }
 
   /**
@@ -197,29 +202,35 @@ public Pose3d getCurrentPose3d() {
    * @return the current pose
    */
   public double[] getCurrentPose3dArray() {
-    Pose3d pose = getCurrentPose3d();
-    Quaternion rotation = pose.getRotation().getQuaternion();
-    return new double[] {
-      pose.getX(),
-      pose.getY(),
-      pose.getZ(),
-      rotation.getW(),
-      rotation.getX(),
-      rotation.getY(),
-      rotation.getZ()
-    };
+    Quaternion rotation = cachedPose3d.getRotation().getQuaternion();
+    pose3dArray[0] = cachedPose3d.getX();
+    pose3dArray[1] = cachedPose3d.getY();
+    pose3dArray[2] = cachedPose3d.getZ();
+    pose3dArray[3] = rotation.getW();
+    pose3dArray[4] = rotation.getX();
+    pose3dArray[5] = rotation.getY();
+    pose3dArray[6] = rotation.getZ();
+    return pose3dArray;
   }
 
   @Override
   public void periodic() {
-    poseProviders.forEach(it -> it.update());
+    for (int i = 0; i < poseProviders.size(); i++) {
+      poseProviders.get(i).update();
+    }
+    // Refresh cached pose BEFORE updatePoseObservationFromProviders() so that getCurrentPose3d()
+    // returns the current cycle's odometry pose for the acceptor distance check and pose reset.
+    cachedPose3d = new Pose3d(poseTracker.getCurrentPose());
     updatePoseObservationFromProviders();
+    // Re-cache after vision fusion so field2d and consumers see the vision-fused pose.
+    cachedPose3d = new Pose3d(poseTracker.getCurrentPose());
     field2d.setRobotPose(getCurrentPose());
   }
 
   private void resetProviderPoses(Pose2d pose) {
-    for (PoseProvider provider : poseProviders) {
-      provider.resetPose(new Pose3d(pose));
+    Pose3d pose3d = new Pose3d(pose);
+    for (int i = 0; i < poseProviders.size(); i++) {
+      poseProviders.get(i).resetPose(pose3d);
     }
   }
 
@@ -239,15 +250,21 @@ protected void updatePoseObservationFromProviders() {
     boolean accepterUpdating = false;
     poseAcceptable = false;
     if (!disableVisionUpdateCommand) {
-      for (PoseProvider provider :
-          poseProviders.stream()
-              .filter(
-                  it ->
-                      it.isConnected()
-                          && (state.type == ProviderType.ALL || it.getType() == state.type))
-              .collect(Collectors.toList())) {
-        List<PoseObservation> observations = provider.getObservations();
-        for (PoseObservation observation : observations) {
+      // Build the filtered provider list without stream/collect allocation
+      activePoseProviders.clear();
+      for (int i = 0; i < poseProviders.size(); i++) {
+        PoseProvider p = poseProviders.get(i);
+        if (p.isConnected() && (state.type == ProviderType.ALL || p.getType() == state.type)) {
+          activePoseProviders.add(p);
+        }
+      }
+      for (int pi = 0; pi < activePoseProviders.size(); pi++) {
+        PoseProvider provider = activePoseProviders.get(pi);
+        PoseObservation[] observations = provider.getObservationsArray();
+        // Cache current pose once per provider to avoid repeated calls inside the observation loop
+        Pose3d cachedCurrentPose3d = getCurrentPose3d();
+        for (int oi = 0; oi < observations.length; oi++) {
+          PoseObservation observation = observations[oi];
           boolean rejectPose =
               (provider.getType() != ProviderType.ENVIRONMENT_BASED)
                       && observation.tagCount() == 0 // Must have at least one tag
@@ -284,7 +301,7 @@ protected void updatePoseObservationFromProviders() {
                       || (!DriverStation.isDisabled()
                           && robotPose
                                   .getTranslation()
-                                  .getDistance(getCurrentPose3d().getTranslation())
+                                  .getDistance(cachedCurrentPose3d.getTranslation())
                               < 0.1));
         }
       }
@@ -292,7 +309,8 @@ protected void updatePoseObservationFromProviders() {
 
     // Accept poses after estimation integration
     if (activateAcceptorUpdates && (poseAcceptable || state == State.DISABLED_FIELD)) {
-      for (PoseProvider provider2 : poseProviders) {
+      for (int i = 0; i < poseProviders.size(); i++) {
+        PoseProvider provider2 = poseProviders.get(i);
         if (provider2.getType() == ProviderType.ENVIRONMENT_BASED) {
           provider2.resetPose(getCurrentPose3d());
           accepterUpdating = true;
diff --git a/src/main/java/org/frc5010/common/drive/pose/PoseProvider.java b/src/main/java/org/frc5010/common/drive/pose/PoseProvider.java
index 9d023423..62604f7e 100644
--- a/src/main/java/org/frc5010/common/drive/pose/PoseProvider.java
+++ b/src/main/java/org/frc5010/common/drive/pose/PoseProvider.java
@@ -10,8 +10,6 @@
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.wpilibj.Alert;
-import edu.wpi.first.wpilibj.Alert.AlertType;
 import java.util.Arrays;
 import java.util.List;
 import org.frc5010.common.vision.VisionConstants;
@@ -20,9 +18,19 @@
 
 public interface PoseProvider {
 
-  public VisionIOInputsAutoLogged input = new VisionIOInputsAutoLogged();
-  public Alert disconnectedAlert = new Alert("PoseProvider", AlertType.kWarning);
-  public int cameraIndex = 0;
+  /**
+   * Returns this provider's own {@link VisionIOInputsAutoLogged} instance.
+   *
+   * <p>Each implementing class must declare and return its own instance. The old pattern of a
+   * single {@code public VisionIOInputsAutoLogged input} field on the interface was implicitly
+   * {@code static final} in Java — meaning all cameras shared one object and each camera's {@code
+   * updateCameraInfo()} overwrote the previous camera's observations.
+   *
+   * @return the per-instance inputs object
+   */
+  public VisionIOInputsAutoLogged getInput();
+
+  public int getCameraIndex();
 
   public enum ProviderType {
     ALL,
@@ -48,6 +56,12 @@ public static class VisionIOInputs {
     public double captureTime;
     public PoseObservation[] poseObservations = new PoseObservation[0];
     public int[] tagIds = new int[0];
+    /** Total summed distance to all visible tags (meters) */
+    public double totalTagDistance = 0.0;
+    /** Pose ambiguity of the best visible target */
+    public double poseAmbiguity = 0.0;
+    /** Latest estimated robot pose from this camera */
+    public Pose3d estimatedRobotPose = new Pose3d();
   }
 
   /** Represents the angle to a simple target, not used for pose estimation. */
@@ -69,7 +83,16 @@ public static record PoseObservation(
    * @return The current observations of the robot.
    */
   public default List<PoseObservation> getObservations() {
-    return Arrays.asList(input.poseObservations);
+    return Arrays.asList(getInput().poseObservations);
+  }
+
+  /**
+   * Returns the raw observations array without wrapping — zero allocation per call.
+   *
+   * @return The current observations of the robot as a raw array.
+   */
+  public default PoseObservation[] getObservationsArray() {
+    return getInput().poseObservations;
   }
 
   /*
@@ -79,11 +102,11 @@ public default List<PoseObservation> getObservations() {
    * @return Whether the pose provider is currently active.
    */
   public default boolean isConnected() {
-    return input.connected;
+    return getInput().connected;
   }
 
   public default double getCaptureTime() {
-    return input.captureTime;
+    return getInput().captureTime;
   }
 
   public void update();
@@ -93,7 +116,7 @@ public default void resetPose(Pose3d initPose) {}
   public ProviderType getType();
 
   public default void logInput(String tableName) {
-    Logger.processInputs(VisionConstants.SBTabVisionDisplay + "/Camera " + tableName, input);
+    Logger.processInputs(VisionConstants.SBTabVisionDisplay + "/Camera " + tableName, getInput());
   }
 
   public default Matrix<N3, N1> getStdDeviations(PoseObservation observation) {
@@ -105,9 +128,9 @@ public default Matrix<N3, N1> getStdDeviations(PoseObservation observation) {
       linearStdDev *= VisionConstants.linearStdDevMegatag2Factor;
       angularStdDev *= VisionConstants.angularStdDevMegatag2Factor;
     }
-    if (cameraIndex < VisionConstants.cameraStdDevFactors.length) {
-      linearStdDev *= VisionConstants.cameraStdDevFactors[cameraIndex];
-      angularStdDev *= VisionConstants.cameraStdDevFactors[cameraIndex];
+    if (getCameraIndex() < VisionConstants.cameraStdDevFactors.length) {
+      linearStdDev *= VisionConstants.cameraStdDevFactors[getCameraIndex()];
+      angularStdDev *= VisionConstants.cameraStdDevFactors[getCameraIndex()];
     }
 
     return VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev);
diff --git a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java
index b206e86a..cbae6d3d 100644
--- a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java
+++ b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java
@@ -120,18 +120,25 @@ public void setupPathPlanner() {
         ppRobotConfigSupplier.get(), // The robot configuration
         () -> {
           // Boolean supplier that controls when the path will be mirrored for the red
-          // alliance
-          // This will flip the path being followed to the red side of the field.
+          // alliance.
           // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-          var alliance = GenericRobot.getAlliance();
-          SmartDashboard.putString("YAGSL Alliance", alliance.toString());
-          return alliance == DriverStation.Alliance.Red;
+          return GenericRobot.getAlliance() == DriverStation.Alliance.Red;
         },
         this // Reference to this subsystem to set requirements
         );
 
-    // Preload PathPlanner Path finding
+    // Preload PathPlanner Path finding.
     // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE
+    warmupPathfinding();
+  }
+
+  /**
+   * Schedules the PathPlanner pathfinding warmup command. Safe to call during robotInit() because
+   * the command is decorated with ignoringDisable(true), which lets it run while the robot is
+   * disabled. Calling this early prevents the ~180 ms overrun that occurs when warmup finishes
+   * mid-match the first time a PathfindingCommand is instantiated.
+   */
+  public void warmupPathfinding() {
     CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand());
   }
 
@@ -144,18 +151,18 @@ public void addAutoCommands(LoggedDashboardChooser<Command> selectableCommand) {
   public void updateGlassWidget() {
     GenericSwerveModuleInfo[] modules = swerveDrive.getModulesInfo();
     for (int moduleKey = 0; moduleKey < modules.length; moduleKey++) {
-      double turningDeg = modules[moduleKey].steerRelativeDegrees();
-      double absEncDeg = modules[moduleKey].steerAbsoluteDegrees();
+      double turningDeg = modules[moduleKey].steerRelativeDegrees;
+      double absEncDeg = modules[moduleKey].steerAbsoluteDegrees;
       // This method will be called once per scheduler run
       absEncDials.get(moduleKey).setAngle(absEncDeg + 90);
       motorDials.get(moduleKey).setAngle(turningDeg + 90);
       motorDials
           .get(moduleKey)
-          .setLength(0.1 * modules[moduleKey].steerVelocityDegreesPerSecond() + 0.02);
+          .setLength(0.1 * modules[moduleKey].steerVelocityDegreesPerSecond + 0.02);
       expectDials
           .get(moduleKey)
-          .setLength(0.1 * modules[moduleKey].driveVelocityMetersPerSecond() + 0.02);
-      expectDials.get(moduleKey).setAngle(modules[moduleKey].expectedSteerDegrees() + 90);
+          .setLength(0.1 * modules[moduleKey].driveVelocityMetersPerSecond + 0.02);
+      expectDials.get(moduleKey).setAngle(modules[moduleKey].expectedSteerDegrees + 90);
     }
   }
 
@@ -572,9 +579,7 @@ public ChassisSpeeds getFieldVelocitiesFromJoystick(
     if (Double.isNaN(xInput)
         || Double.isNaN(yInput)
         || Double.isNaN(turnSpdFunction.getAsDouble())) {
-      SmartDashboard.putBoolean("Controller Overide", true);
-    } else {
-      SmartDashboard.putBoolean("Controller Overide", false);
+      // Input is NaN — fall through to previous-value substitution below
     }
 
     xInput = Double.isNaN(xInput) ? previousLeftXInput : xInput;
diff --git a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java
index fde3b84c..fd371b46 100644
--- a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java
+++ b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java
@@ -7,32 +7,41 @@
 import org.frc5010.common.drive.swerve.akit.Module;
 import swervelib.SwerveModule;
 
-/** Add your docs here. */
-public record GenericSwerveModuleInfo(
-    double steerAbsoluteDegrees,
-    double steerRelativeDegrees,
-    double driveRelativePositionMeters,
-    double driveVelocityMetersPerSecond,
-    double steerVelocityDegreesPerSecond,
-    double expectedSteerDegrees) {
+/**
+ * Mutable container for swerve module telemetry — reused each cycle to avoid per-call allocation.
+ * Use {@link #update(Module)} / {@link #update(SwerveModule)} to refresh values in-place.
+ */
+public class GenericSwerveModuleInfo {
+  public double steerAbsoluteDegrees;
+  public double steerRelativeDegrees;
+  public double driveRelativePositionMeters;
+  public double driveVelocityMetersPerSecond;
+  public double steerVelocityDegreesPerSecond;
+  public double expectedSteerDegrees;
 
-  public GenericSwerveModuleInfo(SwerveModule module) {
-    this(
-        module.getAbsolutePosition(),
-        module.getRelativePosition(),
-        module.getDriveMotor().getPosition(),
-        module.getDriveMotor().getVelocity(),
-        module.getAngleMotor().getVelocity(),
-        module.getState().angle.getDegrees());
+  public GenericSwerveModuleInfo() {}
+
+  /** Update all fields from a YAGSL {@link SwerveModule} — zero allocation after first call. */
+  public void update(SwerveModule module) {
+    steerAbsoluteDegrees = module.getAbsolutePosition();
+    steerRelativeDegrees = module.getRelativePosition();
+    driveRelativePositionMeters = module.getDriveMotor().getPosition();
+    driveVelocityMetersPerSecond = module.getDriveMotor().getVelocity();
+    steerVelocityDegreesPerSecond = module.getAngleMotor().getVelocity();
+    expectedSteerDegrees = module.getState().angle.getDegrees();
   }
 
-  public GenericSwerveModuleInfo(Module module) {
-    this(
-        module.getAngle().getDegrees(),
-        module.getAngle().getDegrees(),
-        module.getPosition().distanceMeters,
-        module.getVelocityMetersPerSec(),
-        0.0,
-        module.getAngle().getDegrees());
+  /**
+   * Update all fields from an AKit {@link Module} — zero allocation after first call. Reads {@code
+   * inputs} values directly to avoid the allocating {@link Module#getPosition()} call.
+   */
+  public void update(Module module) {
+    double angleDeg = module.getAngle().getDegrees();
+    steerAbsoluteDegrees = angleDeg;
+    steerRelativeDegrees = angleDeg;
+    driveRelativePositionMeters = module.getPositionMeters();
+    driveVelocityMetersPerSecond = module.getVelocityMetersPerSec();
+    steerVelocityDegreesPerSecond = 0.0;
+    expectedSteerDegrees = angleDeg;
   }
 }
diff --git a/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java b/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java
index 960992a4..d866c679 100644
--- a/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java
+++ b/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java
@@ -605,9 +605,14 @@ public Field2d getField2d() {
   @Override
   public GenericSwerveModuleInfo[] getModulesInfo() {
     SwerveModule[] modules = swerveDrive.getModules();
-    if (null == moduleInfos) moduleInfos = new GenericSwerveModuleInfo[modules.length];
+    if (null == moduleInfos) {
+      moduleInfos = new GenericSwerveModuleInfo[modules.length];
+      for (int i = 0; i < modules.length; i++) {
+        moduleInfos[i] = new GenericSwerveModuleInfo();
+      }
+    }
     for (int i = 0; i < modules.length; i++) {
-      moduleInfos[i] = new GenericSwerveModuleInfo(modules[i]);
+      moduleInfos[i].update(modules[i]);
     }
     return moduleInfos;
   }
diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/AkitSwerveDrive.java b/src/main/java/org/frc5010/common/drive/swerve/akit/AkitSwerveDrive.java
index 155fc8be..adfae689 100644
--- a/src/main/java/org/frc5010/common/drive/swerve/akit/AkitSwerveDrive.java
+++ b/src/main/java/org/frc5010/common/drive/swerve/akit/AkitSwerveDrive.java
@@ -81,6 +81,27 @@ public class AkitSwerveDrive extends SwerveDriveFunctions {
   private SwerveDrivePoseEstimator poseEstimator;
   private final Consumer<Pose2d> resetSimulationPoseCallBack;
 
+  // Pre-allocated odometry buffers — sized for max odometry samples per 20ms loop.
+  // At 250 Hz odometry the theoretical max is ~5 samples; 16 provides ample headroom.
+  private static final int MAX_ODOMETRY_SAMPLES = 16;
+  /** Empty array sentinel for disabled-mode logging — never mutated */
+  private static final SwerveModuleState[] EMPTY_MODULE_STATES = new SwerveModuleState[] {};
+
+  private final SwerveModulePosition[][] odometryModulePositions =
+      new SwerveModulePosition[MAX_ODOMETRY_SAMPLES][4];
+  private final SwerveModulePosition[][] odometryModuleDeltas =
+      new SwerveModulePosition[MAX_ODOMETRY_SAMPLES][4];
+
+  {
+    // Pre-populate every slot so no null checks are needed in the hot loop
+    for (int s = 0; s < MAX_ODOMETRY_SAMPLES; s++) {
+      for (int m = 0; m < 4; m++) {
+        odometryModulePositions[s][m] = new SwerveModulePosition();
+        odometryModuleDeltas[s][m] = new SwerveModulePosition();
+      }
+    }
+  }
+
   public AkitSwerveDrive(
       AkitSwerveConfig config,
       GyroIO gyroIO,
@@ -145,8 +166,8 @@ public void periodic() {
 
     // Log empty setpoint states when disabled
     if (DriverStation.isDisabled()) {
-      Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
-      Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
+      Logger.recordOutput("SwerveStates/Setpoints", EMPTY_MODULE_STATES);
+      Logger.recordOutput("SwerveStates/SetpointsOptimized", EMPTY_MODULE_STATES);
     }
 
     getChassisSpeeds();
@@ -154,19 +175,20 @@ public void periodic() {
     // Update odometry
     double[] sampleTimestamps =
         modules[0].getOdometryTimestamps(); // All signals are sampled together
-    int sampleCount = sampleTimestamps.length;
+    int sampleCount = Math.min(sampleTimestamps.length, MAX_ODOMETRY_SAMPLES);
     for (int i = 0; i < sampleCount; i++) {
-      // Read wheel positions and deltas from each module
-      SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
-      SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
+      // Reuse pre-allocated position / delta arrays for this sample
+      SwerveModulePosition[] modulePositions = odometryModulePositions[i];
+      SwerveModulePosition[] moduleDeltas = odometryModuleDeltas[i];
       for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
-        modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i];
-        moduleDeltas[moduleIndex] =
-            new SwerveModulePosition(
-                modulePositions[moduleIndex].distanceMeters
-                    - lastModulePositions[moduleIndex].distanceMeters,
-                modulePositions[moduleIndex].angle);
-        lastModulePositions[moduleIndex] = modulePositions[moduleIndex];
+        SwerveModulePosition freshPos = modules[moduleIndex].getOdometryPositions()[i];
+        modulePositions[moduleIndex].distanceMeters = freshPos.distanceMeters;
+        modulePositions[moduleIndex].angle = freshPos.angle;
+        moduleDeltas[moduleIndex].distanceMeters =
+            freshPos.distanceMeters - lastModulePositions[moduleIndex].distanceMeters;
+        moduleDeltas[moduleIndex].angle = freshPos.angle;
+        lastModulePositions[moduleIndex].distanceMeters = freshPos.distanceMeters;
+        lastModulePositions[moduleIndex].angle = freshPos.angle;
       }
 
       // Update gyro angle
@@ -482,9 +504,14 @@ public AngularVelocity getMaximumModuleAngleVelocity() {
 
   @Override
   public GenericSwerveModuleInfo[] getModulesInfo() {
-    if (null == moduleInfos) moduleInfos = new GenericSwerveModuleInfo[modules.length];
+    if (null == moduleInfos) {
+      moduleInfos = new GenericSwerveModuleInfo[modules.length];
+      for (int i = 0; i < modules.length; i++) {
+        moduleInfos[i] = new GenericSwerveModuleInfo();
+      }
+    }
     for (int i = 0; i < modules.length; i++) {
-      moduleInfos[i] = new GenericSwerveModuleInfo(modules[i]);
+      moduleInfos[i].update(modules[i]);
     }
     return moduleInfos;
   }
diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/Module.java b/src/main/java/org/frc5010/common/drive/swerve/akit/Module.java
index b911d5c1..bdf6ef5e 100644
--- a/src/main/java/org/frc5010/common/drive/swerve/akit/Module.java
+++ b/src/main/java/org/frc5010/common/drive/swerve/akit/Module.java
@@ -31,7 +31,8 @@ public class Module {
   private final Alert turnDisconnectedAlert;
   private final Alert turnEncoderDisconnectedAlert;
 
-  private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+  /** Grow-only array: never shrunk to minimise GC pressure from per-cycle allocation */
+  private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[0];
 
   public Module(
       ModuleIO io,
@@ -63,11 +64,17 @@ public void periodic() {
         Math.min(
             inputs.odometryDrivePositionsRad.length,
             inputs.odometryTurnPositions.length); // All signals are sampled together
-    odometryPositions = new SwerveModulePosition[sampleCount];
+    // Grow the array only when needed; never shrink to avoid per-cycle GC pressure
+    if (odometryPositions.length != sampleCount) {
+      odometryPositions = new SwerveModulePosition[sampleCount];
+      for (int i = 0; i < sampleCount; i++) {
+        odometryPositions[i] = new SwerveModulePosition();
+      }
+    }
     for (int i = 0; i < sampleCount; i++) {
-      double positionMeters = inputs.odometryDrivePositionsRad[i] * constants.WheelRadius;
-      Rotation2d angle = inputs.odometryTurnPositions[i];
-      odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+      odometryPositions[i].distanceMeters =
+          inputs.odometryDrivePositionsRad[i] * constants.WheelRadius;
+      odometryPositions[i].angle = inputs.odometryTurnPositions[i];
     }
 
     // Update alerts
diff --git a/src/main/java/org/frc5010/common/sensors/camera/GenericCamera.java b/src/main/java/org/frc5010/common/sensors/camera/GenericCamera.java
index e1efcd78..f576b17e 100644
--- a/src/main/java/org/frc5010/common/sensors/camera/GenericCamera.java
+++ b/src/main/java/org/frc5010/common/sensors/camera/GenericCamera.java
@@ -12,10 +12,13 @@
 import java.util.ArrayList;
 import java.util.List;
 import org.frc5010.common.drive.pose.PoseProvider;
+import org.frc5010.common.drive.pose.VisionIOInputsAutoLogged;
 import org.frc5010.common.vision.VisionConstants;
 
 /** A generic camera interface */
 public abstract class GenericCamera implements PoseProvider {
+  /** Per-instance inputs — each camera has its own object so observations are not overwritten */
+  protected final VisionIOInputsAutoLogged input = new VisionIOInputsAutoLogged();
   /** The list of updaters that will be called every time the camera is updated */
   protected List<Runnable> updaters = new ArrayList<>();
   /** The robot-to-camera transform */
@@ -57,6 +60,18 @@ public GenericCamera(String name, int colIndex, Transform3d robotToCamera) {
     visionLayout.addDouble("Latency", this::getCaptureTime);
   }
 
+  /** {@inheritDoc} Returns this camera's own per-instance inputs object. */
+  @Override
+  public VisionIOInputsAutoLogged getInput() {
+    return input;
+  }
+
+  /** {@inheritDoc} Returns this camera's column index, used for std-dev scaling. */
+  @Override
+  public int getCameraIndex() {
+    return colIndex;
+  }
+
   /**
    * Updates the state of the object by running all registered updaters.
    *
diff --git a/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionCamera.java b/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionCamera.java
index f4e3a61d..30bda0f1 100644
--- a/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionCamera.java
+++ b/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionCamera.java
@@ -38,11 +38,36 @@ public PhotonVisionCamera(String name, int colIndex, Transform3d cameraToRobot)
     camera = new PhotonCamera(name);
   }
 
+  /** The empty pipeline result sentinel — avoids allocating a new one each cycle */
+  private static final PhotonPipelineResult EMPTY_RESULT = new PhotonPipelineResult();
+
+  /**
+   * Maximum number of camera frames to process per 20ms loop cycle.
+   *
+   * <p>PhotonVision buffers every frame received since the NT connection was established. On the
+   * very first call to {@code getAllUnreadResults()} (typically ~16 s after boot), that buffer may
+   * contain hundreds of stale frames (16 s × 30 fps = ~480 frames per camera). Processing all of
+   * them in one cycle causes a 115–400 ms first-periodic overrun. By capping the list to the
+   * most-recent MAX_FRAMES_PER_CYCLE entries we limit latency while still receiving all frames that
+   * arrive during a single 20 ms window (≤ 2 at 30 fps).
+   */
+  private static final int MAX_FRAMES_PER_CYCLE = 10;
+
   /** Update the camera and target with the latest result */
   @Override
   public void updateCameraInfo() {
-    camResults = camera.getAllUnreadResults();
-    camResult = camResults.stream().findFirst().orElse(new PhotonPipelineResult());
+    List<PhotonPipelineResult> allResults = camera.getAllUnreadResults();
+    // Drain stale frames: keep only the most recent MAX_FRAMES_PER_CYCLE results.
+    // This prevents a multi-hundred-millisecond stall on the first periodic() call when
+    // the NT subscriber queue has accumulated hundreds of buffered frames since boot.
+    int size = allResults.size();
+    if (size > MAX_FRAMES_PER_CYCLE) {
+      camResults = allResults.subList(size - MAX_FRAMES_PER_CYCLE, size);
+    } else {
+      camResults = allResults;
+    }
+    // Avoid stream().findFirst() allocation — use direct index access
+    camResult = camResults.isEmpty() ? EMPTY_RESULT : camResults.get(camResults.size() - 1);
     input.connected = camera.isConnected();
     input.captureTime = camResult.getTimestampSeconds();
   }
diff --git a/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionPoseCamera.java b/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionPoseCamera.java
index 4b543da4..3c2bbe04 100644
--- a/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionPoseCamera.java
+++ b/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionPoseCamera.java
@@ -16,7 +16,6 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
 import java.util.HashSet;
 import java.util.List;
@@ -36,6 +35,21 @@ public class PhotonVisionPoseCamera extends PhotonVisionCamera implements Fiduci
   protected Supplier<Pose2d> poseSupplier;
   /** The current list of fiducial IDs */
   protected List<Integer> fiducialIds = new ArrayList<>();
+  /** Pre-allocated list reused each cycle to avoid per-cycle GC pressure */
+  private final List<PoseObservation> observations = new ArrayList<>();
+  /** Pre-allocated set reused each cycle to avoid per-cycle GC pressure */
+  private final Set<Short> tagIds = new HashSet<>();
+  /**
+   * Pre-allocated output arrays — grown on demand but never shrunk to avoid per-cycle allocation
+   */
+  private PoseObservation[] poseObsArray = new PoseObservation[0];
+
+  private int[] tagIdArray = new int[0];
+
+  /** Sentinel empty Pose3d — returned when no estimate is available; never mutated */
+  private static final Pose3d EMPTY_POSE3D = new Pose3d();
+  /** Pre-allocated stdDev matrix — mutated in getStdDeviations() to avoid per-call allocation */
+  private final Matrix<N3, N1> stdDevMatrix = VecBuilder.fill(0.0, 0.0, 0.0);
 
   /**
    * Constructor
@@ -84,14 +98,16 @@ public PhotonVisionPoseCamera(
   public void updateCameraInfo() {
     poseEstimator.addHeadingData(Timer.getFPGATimestamp(), poseSupplier.get().getRotation());
 
-    List<PoseObservation> observations = new ArrayList<>();
-    SmartDashboard.putBoolean("Camera/" + name() + "/updating", true);
+    observations.clear();
+    tagIds.clear();
 
     super.updateCameraInfo();
-    Set<Short> tagIds = new HashSet<>();
+
+    double totalTagDistanceAccum = 0.0;
+    double latestAmbiguity = 0.0;
+    Pose3d latestRobotPose = null;
 
     for (PhotonPipelineResult iCamResult : camResults) {
-      SmartDashboard.putBoolean("Camera/" + name() + "/resuls", iCamResult.hasTargets());
       Optional<EstimatedRobotPose> estimate = poseEstimator.estimateCoprocMultiTagPose(iCamResult);
 
       if (estimate.isEmpty() && !DriverStation.isDisabled()) {
@@ -99,14 +115,6 @@ public void updateCameraInfo() {
       }
 
       if (estimate.isPresent()) {
-        // if (!DriverStation.isDisabled()) {
-        //   Optional<EstimatedRobotPose> finalEstimate =
-        //       poseEstimator.estimatePnpDistanceTrigSolvePose(iCamResult);
-        //   if (finalEstimate.isPresent()) {
-        //     estimate = finalEstimate;
-        //   }
-        // }
-
         EstimatedRobotPose estimatedRobotPose = estimate.get();
         Pose3d robotPose = estimatedRobotPose.estimatedPose;
 
@@ -121,28 +129,15 @@ public void updateCameraInfo() {
           averageDistance = totalTagDistance / iCamResult.targets.size();
         }
 
-        // Add tag IDs
-        iCamResult.multitagResult.map(it -> tagIds.addAll(it.fiducialIDsUsed));
-
-        SmartDashboard.putNumber(
-            "Camera/" + name() + "/Total Distance To Tag " + name, totalTagDistance);
-        SmartDashboard.putNumber(
-            "Camera/" + name() + "/Photon Ambiguity " + name,
-            iCamResult.getBestTarget().poseAmbiguity);
-        SmartDashboard.putNumberArray(
-            "Camera/" + name() + "/Photon Camera " + name + " POSE",
-            new double[] {
-              robotPose.getX(),
-              robotPose.getY(),
-              robotPose.getRotation().toRotation2d().getDegrees()
-            });
-        SmartDashboard.putNumberArray(
-            "Camera/" + name() + "/Photon Camera " + name + " Robot Offset",
-            new double[] {
-              robotToCamera.getX(),
-              robotToCamera.getY(),
-              robotToCamera.getRotation().toRotation2d().getDegrees()
-            });
+        // Add tag IDs — use ifPresent to avoid lambda allocation from .map()
+        if (iCamResult.multitagResult.isPresent()) {
+          tagIds.addAll(iCamResult.multitagResult.get().fiducialIDsUsed);
+        }
+
+        // Accumulate telemetry for AK logging
+        totalTagDistanceAccum += totalTagDistance;
+        latestAmbiguity = iCamResult.getBestTarget().poseAmbiguity;
+        latestRobotPose = robotPose;
 
         observations.add(
             new PoseObservation(
@@ -155,19 +150,33 @@ public void updateCameraInfo() {
                 PoseObservationType.PHOTONVISION,
                 ProviderType.FIELD_BASED));
       }
+    }
 
-      // Save pose observations to inputs object
-      input.poseObservations = new PoseObservation[observations.size()];
-      for (int i = 0; i < observations.size(); i++) {
-        input.poseObservations[i] = observations.get(i);
-      }
-      // Save tag IDs to inputs objects
-      input.tagIds = new int[tagIds.size()];
-      int i = 0;
-      for (int id : tagIds) {
-        input.tagIds[i++] = id;
-      }
+    // Save pose observations to inputs object (once, outside the loop).
+    // Grow the pre-allocated array only when the size increases; never shrink it.
+    int obsSize = observations.size();
+    if (poseObsArray.length != obsSize) {
+      poseObsArray = new PoseObservation[obsSize];
+    }
+    for (int i = 0; i < obsSize; i++) {
+      poseObsArray[i] = observations.get(i);
+    }
+    input.poseObservations = poseObsArray;
+
+    // Save tag IDs to inputs object (once, outside the loop).
+    int tagCount2 = tagIds.size();
+    if (tagIdArray.length != tagCount2) {
+      tagIdArray = new int[tagCount2];
+    }
+    int i = 0;
+    for (int id : tagIds) {
+      tagIdArray[i++] = id;
     }
+    input.tagIds = tagIdArray;
+    // Log telemetry fields via AdvantageKit inputs
+    input.totalTagDistance = totalTagDistanceAccum;
+    input.poseAmbiguity = latestAmbiguity;
+    input.estimatedRobotPose = latestRobotPose != null ? latestRobotPose : EMPTY_POSE3D;
   }
 
   @Override
@@ -192,7 +201,10 @@ public Matrix<N3, N1> getStdDeviations(PoseObservation observation) {
         && observation.tagCount() < 2)) {
       linearStdDev = 100.0;
     }
-    return VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev);
+    stdDevMatrix.set(0, 0, linearStdDev);
+    stdDevMatrix.set(1, 0, linearStdDev);
+    stdDevMatrix.set(2, 0, angularStdDev);
+    return stdDevMatrix;
   }
 
   /**
diff --git a/src/main/java/org/frc5010/common/sensors/camera/QuestNavInterface.java b/src/main/java/org/frc5010/common/sensors/camera/QuestNavInterface.java
index 7233a47f..d1e6d6d4 100644
--- a/src/main/java/org/frc5010/common/sensors/camera/QuestNavInterface.java
+++ b/src/main/java/org/frc5010/common/sensors/camera/QuestNavInterface.java
@@ -26,10 +26,14 @@
 import java.util.function.Supplier;
 import org.frc5010.common.drive.GenericDrivetrain;
 import org.frc5010.common.drive.pose.PoseProvider;
+import org.frc5010.common.drive.pose.VisionIOInputsAutoLogged;
 
 /** Add your docs here. */
 public class QuestNavInterface implements PoseProvider {
 
+  /** Per-instance inputs — each QuestNav has its own object */
+  private final VisionIOInputsAutoLogged input = new VisionIOInputsAutoLogged();
+
   private String networkTableRoot = "questnav";
   private Supplier<ChassisSpeeds> robotVelocity = null;
   private Transform3d robotToQuest;
@@ -52,6 +56,13 @@ public class QuestNavInterface implements PoseProvider {
   private Translation2d _calculatedOffsetToRobotCenter = new Translation2d();
   private int _calculatedOffsetToRobotCenterCount = 0;
 
+  /** Reusable observation list — cleared each cycle to avoid per-cycle ArrayList allocation */
+  private final List<PoseObservation> questObservations = new ArrayList<>();
+  /** Grow-only output array — never shrunk to minimise GC pressure */
+  private PoseObservation[] questObsArray = new PoseObservation[0];
+  /** Pre-allocated stdDev matrix — mutated in getStdDeviations() to avoid per-call allocation */
+  private final Matrix<N3, N1> stdDevMatrix = VecBuilder.fill(0.0, 0.0, 0.0);
+
   public QuestNavInterface(Transform3d robotToQuest) {
     super();
     this.robotToQuest = robotToQuest;
@@ -65,6 +76,16 @@ public QuestNavInterface(Transform3d robotToQuest, String networkTableRoot) {
     this.questNav = new QuestNav();
   }
 
+  @Override
+  public VisionIOInputsAutoLogged getInput() {
+    return input;
+  }
+
+  @Override
+  public int getCameraIndex() {
+    return 0;
+  }
+
   private Pose3d getRobotPoseFromQuestPose(Pose3d questPose) {
     return questPose.transformBy(robotToQuest.inverse());
   }
@@ -119,14 +140,14 @@ private void updateObservations() {
       latestPoseFrame = unreadQuestFrames[unreadQuestFrames.length - 1];
     }
 
-    List<PoseObservation> observations = new ArrayList<>();
+    questObservations.clear();
 
     if (initializedPosition) {
       for (PoseFrame frame : unreadQuestFrames) {
         Pose3d robotPose =
             getRobotPoseFromQuestPose(frame.questPose3d()).transformBy(softResetTransform);
         double captureTime = frame.dataTimestamp();
-        observations.add(
+        questObservations.add(
             new PoseObservation(
                 captureTime,
                 robotPose,
@@ -138,11 +159,15 @@ private void updateObservations() {
       }
     }
     input.connected = isActive();
-    // Save pose observations to inputs object
-    input.poseObservations = new PoseObservation[observations.size()];
-    for (int i = 0; i < observations.size(); i++) {
-      input.poseObservations[i] = observations.get(i);
+    // Save pose observations to inputs object using grow-only pre-allocated array
+    int obsSize = questObservations.size();
+    if (questObsArray.length != obsSize) {
+      questObsArray = new PoseObservation[obsSize];
+    }
+    for (int i = 0; i < obsSize; i++) {
+      questObsArray[i] = questObservations.get(i);
     }
+    input.poseObservations = questObsArray;
   }
 
   @Override
@@ -161,7 +186,10 @@ public Matrix<N3, N1> getStdDeviations(PoseObservation observation) {
         calib = 10;
       }
     }
-    return VecBuilder.fill(calib, calib, calib * 0.2);
+    stdDevMatrix.set(0, 0, calib);
+    stdDevMatrix.set(1, 0, calib);
+    stdDevMatrix.set(2, 0, calib * 0.2);
+    return stdDevMatrix;
   }
 
   public double getConfidence() {
diff --git a/src/main/java/org/frc5010/common/sensors/camera/QuestNavOld.java b/src/main/java/org/frc5010/common/sensors/camera/QuestNavOld.java
index ca2a5a25..c86278b1 100644
--- a/src/main/java/org/frc5010/common/sensors/camera/QuestNavOld.java
+++ b/src/main/java/org/frc5010/common/sensors/camera/QuestNavOld.java
@@ -40,10 +40,17 @@
 import org.frc5010.common.drive.pose.DrivePoseEstimator;
 import org.frc5010.common.drive.pose.DrivePoseEstimator.State;
 import org.frc5010.common.drive.pose.PoseProvider;
+import org.frc5010.common.drive.pose.VisionIOInputsAutoLogged;
 import org.frc5010.common.drive.swerve.GenericSwerveDrivetrain;
 
 /** Add your docs here. */
 public class QuestNavOld implements PoseProvider {
+  /** Per-instance inputs — each QuestNav has its own object */
+  private final VisionIOInputsAutoLogged input = new VisionIOInputsAutoLogged();
+  /** Per-instance disconnected alert */
+  private final edu.wpi.first.wpilibj.Alert disconnectedAlert =
+      new edu.wpi.first.wpilibj.Alert("QuestNav", edu.wpi.first.wpilibj.Alert.AlertType.kWarning);
+
   private boolean initializedPosition = false;
   public static boolean isActive = false;
   private String networkTableRoot = "questnav";
@@ -115,6 +122,16 @@ public QuestNavOld(Transform3d robotToQuest, String networkTableRoot) {
     setupInitialTimestamp();
   }
 
+  @Override
+  public VisionIOInputsAutoLogged getInput() {
+    return input;
+  }
+
+  @Override
+  public int getCameraIndex() {
+    return 0;
+  }
+
   private void setupInitialTimestamp() {
     startTimestamp = timestamp.get();
   }
diff --git a/src/main/java/org/frc5010/common/subsystems/CameraSystem.java b/src/main/java/org/frc5010/common/subsystems/CameraSystem.java
index 2e62c114..0abb367c 100644
--- a/src/main/java/org/frc5010/common/subsystems/CameraSystem.java
+++ b/src/main/java/org/frc5010/common/subsystems/CameraSystem.java
@@ -54,6 +54,10 @@ public abstract class CameraSystem extends GenericSubsystem {
   protected TargetModel targetModel = new TargetModel(0.3556);
   /** Whether to view game pieces in simulation */
   protected boolean viewGamePieces = true;
+  /** Cached game piece A list to detect changes between cycles */
+  private List<Pose3d> cachedGpas = List.of();
+  /** Cached game piece B list to detect changes between cycles */
+  private List<Pose3d> cachedGpbs = List.of();
 
   /**
    * Creates a new CameraSystem with the specified camera.
@@ -88,25 +92,31 @@ public void simulationPeriodic() {
     if (!camera.canViewGamePieces()) {
       return;
     }
-    // Update game piece A targets in simulation
+    // Update game piece A targets only when the list has changed
     List<Pose3d> gpas =
         SimulatedArena.getInstance().getGamePiecesByType(Constants.Simulation.gamePieceA).stream()
             .map(it -> it.getPose3d())
             .collect(Collectors.toList());
-    SimulatedCamera.visionSim.removeVisionTargets("GPA");
-    for (Pose3d gpa : gpas) {
-      VisionTargetSim simTarget = new VisionTargetSim(gpa, targetModel);
-      SimulatedCamera.visionSim.addVisionTargets("GPA", simTarget);
+    if (!gpas.equals(cachedGpas)) {
+      cachedGpas = gpas;
+      SimulatedCamera.visionSim.removeVisionTargets("GPA");
+      for (Pose3d gpa : gpas) {
+        VisionTargetSim simTarget = new VisionTargetSim(gpa, targetModel);
+        SimulatedCamera.visionSim.addVisionTargets("GPA", simTarget);
+      }
     }
-    // Update game piece B targets in simulation
+    // Update game piece B targets only when the list has changed
     List<Pose3d> gpbs =
         SimulatedArena.getInstance().getGamePiecesByType(Constants.Simulation.gamePieceB).stream()
             .map(it -> it.getPose3d())
             .collect(Collectors.toList());
-    SimulatedCamera.visionSim.removeVisionTargets("GPB");
-    for (Pose3d gpb : gpbs) {
-      VisionTargetSim simTarget = new VisionTargetSim(gpb, targetModel);
-      SimulatedCamera.visionSim.addVisionTargets("GPB", simTarget);
+    if (!gpbs.equals(cachedGpbs)) {
+      cachedGpbs = gpbs;
+      SimulatedCamera.visionSim.removeVisionTargets("GPB");
+      for (Pose3d gpb : gpbs) {
+        VisionTargetSim simTarget = new VisionTargetSim(gpb, targetModel);
+        SimulatedCamera.visionSim.addVisionTargets("GPB", simTarget);
+      }
     }
   }
 
diff --git a/src/main/java/org/frc5010/common/utils/OrchestraManager.java b/src/main/java/org/frc5010/common/utils/OrchestraManager.java
new file mode 100644
index 00000000..4b93d066
--- /dev/null
+++ b/src/main/java/org/frc5010/common/utils/OrchestraManager.java
@@ -0,0 +1,68 @@
+package org.frc5010.common.utils;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.Orchestra;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.configs.AudioConfigs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj.Filesystem;
+import java.util.HashMap;
+import java.util.Map;
+import org.frc5010.common.config.json.devices.OrchestraConfigJson;
+
+/** Manages CTRE Orchestra playback through all robot TalonFX motors during disabled mode. */
+public class OrchestraManager {
+  private static Orchestra orchestra;
+  private static Map<String, String> musicMap;
+  private static OrchestraConfigJson musicConfigJson;
+
+  public static void init(OrchestraConfigJson configJson) {
+    orchestra = new Orchestra();
+    musicMap = new HashMap<>();
+    musicConfigJson = configJson;
+    for (OrchestraConfigJson.MusicEntry entry : musicConfigJson.music) {
+      musicMap.put(entry.name, entry.path);
+    }
+  }
+
+  public static void loadMusic(String musicFileName) {
+    if (null != orchestra) {
+      for (int id : musicConfigJson.rioIds) {
+        TalonFX motor = new TalonFX(id);
+        AudioConfigs config = new AudioConfigs().withAllowMusicDurDisable(true);
+        motor.getConfigurator().apply(config, 0);
+        orchestra.addInstrument(motor);
+      }
+      CANBus canivoreBus = new CANBus("canivore");
+      for (int id : musicConfigJson.canivoreIds) {
+        TalonFX motor = new TalonFX(id, canivoreBus);
+        AudioConfigs config = new AudioConfigs().withAllowMusicDurDisable(true);
+        motor.getConfigurator().apply(config, 0);
+        orchestra.addInstrument(motor, 0);
+      }
+      orchestra.loadMusic(
+          Filesystem.getDeployDirectory().toPath().resolve(musicMap.get(musicFileName)).toString());
+    }
+  }
+
+  /** Start playing the mariachi song. Safe to call repeatedly. */
+  public static void play() {
+    if (null != orchestra) {
+      StatusCode status = orchestra.play();
+      if (!status.isOK()) {
+        System.err.println("Failed to play music: " + status);
+      }
+    }
+  }
+
+  public static boolean isPlaying() {
+    return null != orchestra && orchestra.isPlaying();
+  }
+
+  /** Stop playback and release motors back to normal control. */
+  public static void stop() {
+    if (null != orchestra) {
+      orchestra.stop();
+    }
+  }
+}
diff --git a/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnField.java b/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnField.java
deleted file mode 100644
index d00e3f82..00000000
--- a/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnField.java
+++ /dev/null
@@ -1,21 +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 org.frc5010.lobbinloco;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation;
-
-/** Add your docs here. */
-public class FRC5010BallOnField extends GamePieceOnFieldSimulation {
-  public FRC5010BallOnField() {
-    super(LobbinLoco.LOBBINLOCO_BALL_INFO, new Pose2d(0, 0, new Rotation2d()));
-  }
-
-  public FRC5010BallOnField(Translation2d initialPosition) {
-    super(LobbinLoco.LOBBINLOCO_BALL_INFO, new Pose2d(initialPosition, new Rotation2d()));
-  }
-}
diff --git a/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnTheFly.java b/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnTheFly.java
deleted file mode 100644
index 3a656088..00000000
--- a/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnTheFly.java
+++ /dev/null
@@ -1,48 +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 org.frc5010.lobbinloco;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.units.measure.LinearVelocity;
-import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceProjectile;
-
-/** Add your docs here. */
-public class FRC5010BallOnTheFly extends GamePieceProjectile {
-  private static Runnable hitBoxCallBack = () -> System.out.println("hit target!");
-
-  public static void setHitBoxCallBack(Runnable callBack) {
-    hitBoxCallBack = callBack;
-  }
-
-  public FRC5010BallOnTheFly(
-      Translation2d robotPosition,
-      Translation2d shooterPositionOnRobot,
-      ChassisSpeeds chassisSpeeds,
-      Rotation2d shooterFacing,
-      Distance initialHeight,
-      LinearVelocity launchingSpeed,
-      Angle shooterAngle) {
-    super(
-        LobbinLoco.LOBBINLOCO_BALL_INFO,
-        robotPosition,
-        shooterPositionOnRobot,
-        chassisSpeeds,
-        shooterFacing,
-        initialHeight,
-        launchingSpeed,
-        shooterAngle);
-    super.withTouchGroundHeight(0.8);
-    super.enableBecomesGamePieceOnFieldAfterTouchGround();
-    super.withTargetTolerance(
-        new Translation3d(LobbinLoco.GOAL_LENGTH, LobbinLoco.GOAL_WIDTH, LobbinLoco.BALL_HEIGHT)
-            .div(2.0));
-    super.withHitTargetCallBack(hitBoxCallBack);
-  }
-}
diff --git a/src/main/java/org/frc5010/lobbinloco/LobbinLoco.java b/src/main/java/org/frc5010/lobbinloco/LobbinLoco.java
deleted file mode 100644
index 6c6282b2..00000000
--- a/src/main/java/org/frc5010/lobbinloco/LobbinLoco.java
+++ /dev/null
@@ -1,116 +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 org.frc5010.lobbinloco;
-
-import static edu.wpi.first.units.Units.Inches;
-import static edu.wpi.first.units.Units.Kilograms;
-import static edu.wpi.first.units.Units.Meters;
-
-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.units.Units;
-import org.dyn4j.geometry.Circle;
-import swervelib.simulation.ironmaple.simulation.SimulatedArena;
-import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation.GamePieceInfo;
-
-/** Add your docs here. */
-public class LobbinLoco extends SimulatedArena {
-  public static final double FIELD_LENGTH = Units.Feet.of(16).in(Units.Meters);
-  public static final double FIELD_OVERALL_LENGTH = Units.Feet.of(50).in(Units.Meters);
-  public static final double FIELD_WIDTH = Units.Feet.of(20).in(Units.Meters);
-  public static final double GOAL_LENGTH = Inches.of(36).in(Meters);
-  public static final double GOAL_WIDTH = Inches.of(36).in(Meters);
-  public static final double GOAL_HEIGHT = Inches.of(8).in(Meters);
-  public static final double BALL_HEIGHT = Inches.of(5).in(Meters);
-
-  // Dimensions in meters (50 feet x 20 feet)
-  private static final Translation2d bottomLeft = new Translation2d(0.0, 0.0);
-  private static final Translation2d bottomRight = new Translation2d(FIELD_LENGTH, 0);
-  private static final Translation2d bottomRightOverall =
-      new Translation2d(FIELD_OVERALL_LENGTH, 0);
-  private static final Translation2d topLeft = new Translation2d(0.0, FIELD_WIDTH);
-  private static final Translation2d topRight = new Translation2d(FIELD_LENGTH, FIELD_WIDTH);
-  private static final Translation2d topRightOverall =
-      new Translation2d(FIELD_OVERALL_LENGTH, FIELD_WIDTH);
-
-  public static final GamePieceInfo LOBBINLOCO_BALL_INFO =
-      new GamePieceInfo(
-          "FRC5010Ball",
-          new Circle(Inches.of(5).in(Meters)),
-          Inches.of(5),
-          Kilograms.of(0.1),
-          1.8,
-          5,
-          0.8);
-
-  private static class LobbinLoboFieldMap extends FieldMap {
-    public LobbinLoboFieldMap() {
-      addBorderLine(bottomLeft, bottomRight); // _
-      addBorderLine(bottomRight, bottomRightOverall); // __
-      addBorderLine(bottomRight, topRight); // _|_
-      addBorderLine(bottomRightOverall, topRightOverall); // _|_|
-      addBorderLine(topRight, topRightOverall); //
-      addBorderLine(topRight, topLeft);
-      addBorderLine(topLeft, bottomLeft);
-
-      defineGoal(288, 30); // 18
-      defineGoal(288, 170); // 17
-      defineGoal(432, 80); // 20
-      defineGoal(432, 120); // 19
-      defineGoal(528, 10); // 22
-      defineGoal(528, 190); // 21
-    }
-
-    private void defineGoal(double x, double y) {
-      double xInMeters = Inches.of(x).in(Meters);
-      double yInMeters = Inches.of(y).in(Meters);
-      // Goal Right Wall
-      // this.addBorderLine(
-      // new Translation2d(xInMeters, yInMeters),
-      // new Translation2d(xInMeters + GOAL_LENGTH, yInMeters));
-      this.addRectangularObstacle(
-          GOAL_LENGTH, GOAL_HEIGHT, new Pose2d(xInMeters, yInMeters, new Rotation2d()));
-      // Goal Front Wall
-      // this.addBorderLine(
-      // new Translation2d(xInMeters, yInMeters),
-      // new Translation2d(xInMeters, yInMeters + GOAL_WIDTH));
-      this.addRectangularObstacle(
-          GOAL_LENGTH, GOAL_HEIGHT, new Pose2d(xInMeters, yInMeters, new Rotation2d(90.0)));
-      // Goal Left Wall
-      // this.addBorderLine(
-      // new Translation2d(xInMeters, yInMeters + GOAL_WIDTH),
-      // new Translation2d(xInMeters + GOAL_LENGTH, yInMeters + GOAL_WIDTH));
-      this.addRectangularObstacle(
-          GOAL_LENGTH,
-          GOAL_HEIGHT,
-          new Pose2d(xInMeters, yInMeters + GOAL_WIDTH, new Rotation2d()));
-      // Goal Back Wall
-      // this.addBorderLine(
-      // new Translation2d(xInMeters + GOAL_LENGTH, yInMeters),
-      // new Translation2d(xInMeters + GOAL_LENGTH, yInMeters + GOAL_WIDTH));
-      this.addRectangularObstacle(
-          GOAL_LENGTH,
-          GOAL_HEIGHT,
-          new Pose2d(xInMeters + GOAL_LENGTH, yInMeters + GOAL_WIDTH, new Rotation2d(-90.0)));
-    }
-  }
-
-  private static final LobbinLoboFieldMap fieldMap = new LobbinLoboFieldMap();
-
-  public LobbinLoco() {
-    super(fieldMap);
-  }
-
-  /**
-   * Places game pieces on the field according to the current game configuration. This method is
-   * called by the SimulatedArena class when the game starts. The method is responsible for placing
-   * the game pieces in the correct positions on the simulated field.
-   */
-  @Override
-  public void placeGamePiecesOnField() {
-    // no pieces on the field to start
-  }
-}
diff --git a/src/test/java/org/frc5010/common/motors/SystemIdentificationTest.java b/src/test/java/org/frc5010/common/motors/SystemIdentificationTest.java
new file mode 100644
index 00000000..665b5e17
--- /dev/null
+++ b/src/test/java/org/frc5010/common/motors/SystemIdentificationTest.java
@@ -0,0 +1,78 @@
+// 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 org.frc5010.common.motors;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class SystemIdentificationTest {
+
+  @Test
+  void testComputeFeedforwardCoefficientsWithAcceleration() {
+    double kS = 0.2;
+    double kV = 1.5;
+    double kA = 0.3;
+    double dt = 0.02;
+
+    List<Double> velocitySamples = new ArrayList<>();
+    List<Double> voltageSamples = new ArrayList<>();
+    List<Double> timeSamples = new ArrayList<>();
+
+    double time = 0.0;
+    double velocity = 0.0;
+    for (int i = 0; i < 120; i++) {
+      double accel = 0.6 + 0.25 * Math.sin(i * 0.15);
+      velocity += accel * dt;
+      double voltage = kS + kV * velocity + kA * accel;
+      velocitySamples.add(velocity);
+      voltageSamples.add(voltage);
+      timeSamples.add(time);
+      time += dt;
+    }
+
+    double[] coefficients =
+        SystemIdentification.computeFeedforwardCoefficients(
+            velocitySamples, voltageSamples, timeSamples);
+
+    assertNotNull(coefficients);
+    assertEquals(kS, coefficients[0], 1e-2);
+    assertEquals(kV, coefficients[1], 1e-2);
+    assertEquals(kA, coefficients[2], 1e-2);
+  }
+
+  @Test
+  void testComputeFeedforwardCoefficientsWithoutAccelerationData() {
+    double kS = 0.1;
+    double kV = 2.0;
+    double dt = 0.02;
+
+    List<Double> velocitySamples = new ArrayList<>();
+    List<Double> voltageSamples = new ArrayList<>();
+    List<Double> timeSamples = new ArrayList<>();
+
+    double velocity = 0.0;
+    double time = 0.0;
+    for (int i = 0; i < 30; i++) {
+      double voltage = kS + kV * velocity;
+      velocitySamples.add(velocity);
+      voltageSamples.add(voltage);
+      timeSamples.add(time);
+      velocity += 0.25;
+      time += dt;
+    }
+
+    double[] coefficients =
+        SystemIdentification.computeFeedforwardCoefficients(velocitySamples, voltageSamples, null);
+
+    assertNotNull(coefficients);
+    assertEquals(kS, coefficients[0], 1e-6);
+    assertEquals(kV, coefficients[1], 1e-6);
+    assertEquals(0.0, coefficients[2], 1e-9);
+  }
+}