diff --git a/build.gradle b/build.gradle
index 06ca256..f91cefd 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,17 @@
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2026.2.1"
+    id "com.peterabeles.gversion" version "1.10"
+}
+
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+  srcDir       = "src/main/java/"
+  classPackage = "frc.robot"
+  className    = "BuildConstants"
+  dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+  timeZone     = "America/New_York" // Use preferred time zone
+  indent       = "  "
 }
 
 java {
@@ -27,6 +38,15 @@ deploy {
                 // getTargetTypeClass is a shortcut to get the class type using a string
 
                 frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                    // Enable VisualVM connection
+                    jvmArgs.add("-Dcom.sun.management.jmxremote=true")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
+                    jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number
+                    jvmArgs.add("-XX:+HeapDumpOnOutOfMemoryError")
+                    jvmArgs.add("-XX:HeapDumpPath=/home/lvuser/dumps/")
                 }
 
                 // Static files artifact
@@ -50,13 +70,24 @@ wpi.java.debugJni = false
 // Set this to true to enable desktop support.
 def includeDesktopSupport = true
 
+
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.
+
+task(replayWatch, type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.ReplayWatch"
+    classpath = sourceSets.main.runtimeClasspath
+}
+
 dependencies {
     annotationProcessor wpi.java.deps.wpilibAnnotations()
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 
+    // ...
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
+
     roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
     roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
 
@@ -75,6 +106,41 @@ dependencies {
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 }
 
+
+
+// Create commit with working changes on event branches
+task(eventDeploy) {
+    doLast {
+        if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) {
+            def branchPrefix = "event"
+            def branch = 'git branch --show-current'.execute().text.trim()
+            def commitMessage = "Update at '${new Date().toString()}'"
+
+            if (branch.startsWith(branchPrefix)) {
+                exec {
+                    workingDir(projectDir)
+                    executable 'git'
+                    args 'add', '-A'
+                }
+                exec {
+                    workingDir(projectDir)
+                    executable 'git'
+                    args 'commit', '-m', commitMessage
+                    ignoreExitValue = true
+                }
+
+                println "Committed to branch: '$branch'"
+                println "Commit message: '$commitMessage'"
+            } else {
+                println "Not on an event branch, skipping commit"
+            }
+        } else {
+            println "Not running deploy task, skipping commit"
+        }
+    }
+}
+createVersionFile.dependsOn(eventDeploy)
+
 test {
     useJUnitPlatform()
     systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
diff --git a/simgui-ds.json b/simgui-ds.json
index 0bea79e..b9cdd1c 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -20,6 +20,14 @@
           "keyRate": 0.009999999776482582
         },
         {},
+        {
+          "decKey": 49,
+          "incKey": 50
+        },
+        {
+          "decKey": 51,
+          "incKey": 52
+        },
         {
           "decKey": 74,
           "incKey": 76
@@ -61,7 +69,7 @@
       "axisCount": 2,
       "buttonCount": 4,
       "buttonKeys": [
-        77,
+        -1,
         44,
         46,
         47
diff --git a/src/main/deploy/choreo/rightTrench.traj b/src/main/deploy/choreo/rightTrench.traj
new file mode 100644
index 0000000..6d56648
--- /dev/null
+++ b/src/main/deploy/choreo/rightTrench.traj
@@ -0,0 +1,27 @@
+{
+ "name":"rightTrench",
+ "version":1,
+ "snapshot":{
+  "waypoints":[],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":null,
+  "waypoints":[],
+  "samples":[],
+  "splits":[]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/testPath3.traj b/src/main/deploy/choreo/testPath3.traj
deleted file mode 100644
index 5f72622..0000000
--- a/src/main/deploy/choreo/testPath3.traj
+++ /dev/null
@@ -1,403 +0,0 @@
-{
- "name":"testPath3",
- "version":1,
- "snapshot":{
-  "waypoints":[
-    {"x":7.125288963317871, "y":1.5220201015472412, "heading":2.297438401528901, "intervals":58, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":3.6629953384399414, "y":3.0058600902557373, "heading":1.0010398833246623, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":1.1074931621551514, "y":0.9861889481544496, "heading":0.8709035133255352, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":3.951519966125488, "y":2.7791624069213867, "heading":1.0427221780766034, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":1.0868842601776123, "y":1.0067977905273438, "heading":0.8818720101391697, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":3.147773265838623, "y":3.87143349647522, "heading":0.0, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":1.010371446609497, "y":6.991826057434082, "heading":-0.9272954961807792, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":3.684423923492432, "y":5.05485200881958, "heading":-1.0240075818295364, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
-  "constraints":[
-    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
-    {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":7, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":6, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}],
-  "targetDt":0.05
- },
- "params":{
-  "waypoints":[
-    {"x":{"exp":"7.125288963317871 m", "val":7.125288963317871}, "y":{"exp":"1.5220201015472412 m", "val":1.5220201015472412}, "heading":{"exp":"2.297438401528901 rad", "val":2.297438401528901}, "intervals":58, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"3.6629953384399414 m", "val":3.6629953384399414}, "y":{"exp":"3.0058600902557373 m", "val":3.0058600902557373}, "heading":{"exp":"1.0010398833246623 rad", "val":1.0010398833246623}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"1.1074931621551514 m", "val":1.1074931621551514}, "y":{"exp":"0.9861889481544495 m", "val":0.9861889481544496}, "heading":{"exp":"0.8709035133255352 rad", "val":0.8709035133255352}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"3.9515199661254883 m", "val":3.951519966125488}, "y":{"exp":"2.7791624069213867 m", "val":2.7791624069213867}, "heading":{"exp":"1.0427221780766034 rad", "val":1.0427221780766034}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"1.0868842601776123 m", "val":1.0868842601776123}, "y":{"exp":"1.0067977905273438 m", "val":1.0067977905273438}, "heading":{"exp":"0.8818720101391697 rad", "val":0.8818720101391697}, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"3.147773265838623 m", "val":3.147773265838623}, "y":{"exp":"3.8714334964752197 m", "val":3.87143349647522}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"1.010371446609497 m", "val":1.010371446609497}, "y":{"exp":"6.991826057434082 m", "val":6.991826057434082}, "heading":{"exp":"-0.9272954961807793 rad", "val":-0.9272954961807792}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"3.6844239234924316 m", "val":3.684423923492432}, "y":{"exp":"5.05485200881958 m", "val":5.05485200881958}, "heading":{"exp":"-1.0240075818295364 rad", "val":-1.0240075818295364}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
-  "constraints":[
-    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
-    {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":7, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":6, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}],
-  "targetDt":{
-   "exp":"0.05 s",
-   "val":0.05
-  }
- },
- "trajectory":{
-  "sampleType":"Swerve",
-  "waypoints":[0.0,2.07522,3.9942,5.94381,7.89686,9.90077,11.97542,13.90891],
-  "samples":[
-    {"t":0.0, "x":7.12529, "y":1.52202, "heading":2.29744, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.21938, "ay":1.38054, "alpha":-1.19736, "fx":[-40.35714,-45.25012,-47.08586,-42.54132], "fy":[26.24058,16.43546,9.97559,22.49251]},
-    {"t":0.03578, "x":7.12323, "y":1.5229, "heading":2.29744, "vx":-0.11519, "vy":0.0494, "omega":-0.04284, "ax":-3.21923, "ay":1.38047, "alpha":-1.19772, "fx":[-40.35437,-45.24878,-47.08442,-42.5385], "fy":[26.24145,16.4342,9.97221,22.49258]},
-    {"t":0.07156, "x":7.11705, "y":1.52555, "heading":2.29591, "vx":-0.23037, "vy":0.09879, "omega":-0.0857, "ax":-3.21907, "ay":1.3804, "alpha":-1.1979, "fx":[-40.3479,-45.24331,-47.08328,-42.54309], "fy":[26.24771,16.44395,9.9667,22.4782]},
-    {"t":0.10734, "x":7.10674, "y":1.52997, "heading":2.29284, "vx":-0.34555, "vy":0.14818, "omega":-0.12856, "ax":-3.21891, "ay":1.38032, "alpha":-1.19789, "fx":[-40.33775,-45.23368,-47.08242,-42.5551], "fy":[26.25932,16.46473,9.95913,22.44929]},
-    {"t":0.14312, "x":7.09232, "y":1.53616, "heading":2.28824, "vx":-0.46072, "vy":0.19757, "omega":-0.17142, "ax":-3.21875, "ay":1.38024, "alpha":-1.19769, "fx":[-40.32392,-45.21984,-47.08179,-42.57453], "fy":[26.2762,16.49654,9.94965,22.40572]},
-    {"t":0.1789, "x":7.07377, "y":1.54411, "heading":2.28211, "vx":-0.57589, "vy":0.24695, "omega":-0.21427, "ax":-3.21858, "ay":1.38016, "alpha":-1.19731, "fx":[-40.30648,-45.20175,-47.08131,-42.60137], "fy":[26.29821,16.5394,9.93846,22.34733]},
-    {"t":0.21468, "x":7.05111, "y":1.55383, "heading":2.27444, "vx":-0.69104, "vy":0.29633, "omega":-0.25711, "ax":-3.21841, "ay":1.38006, "alpha":-1.19676, "fx":[-40.28547,-45.17933,-47.0809,-42.63562], "fy":[26.3252,16.59335,9.92584,22.2739]},
-    {"t":0.25046, "x":7.02432, "y":1.56532, "heading":2.26524, "vx":-0.8062, "vy":0.34571, "omega":-0.29993, "ax":-3.21822, "ay":1.37996, "alpha":-1.19602, "fx":[-40.26099,-45.1525,-47.08046,-42.67727], "fy":[26.35695,16.65839,9.91213,22.18516]},
-    {"t":0.28624, "x":6.99342, "y":1.57857, "heading":2.25451, "vx":-0.92134, "vy":0.39508, "omega":-0.34272, "ax":-3.21802, "ay":1.37984, "alpha":-1.19513, "fx":[-40.23313,-45.12116,-47.07986,-42.72631], "fy":[26.39319,16.73454,9.89776,22.08084]},
-    {"t":0.32202, "x":6.95839, "y":1.59359, "heading":2.24225, "vx":-1.03648, "vy":0.44445, "omega":-0.38548, "ax":-3.21781, "ay":1.37971, "alpha":-1.19407, "fx":[-40.20202,-45.08521,-47.07894,-42.78268], "fy":[26.43361,16.82181,9.88319,21.96058]},
-    {"t":0.3578, "x":6.91925, "y":1.61037, "heading":2.22845, "vx":-1.15162, "vy":0.49382, "omega":-0.42821, "ax":-3.21758, "ay":1.37956, "alpha":-1.19288, "fx":[-40.16781,-45.04449,-47.07755,-42.84634], "fy":[26.47787,16.92022,9.86896,21.82404]},
-    {"t":0.39358, "x":6.87599, "y":1.62892, "heading":2.21313, "vx":-1.26674, "vy":0.54318, "omega":-0.47089, "ax":-3.21732, "ay":1.37939, "alpha":-1.19156, "fx":[-40.13065,-44.99888,-47.07549,-42.91717], "fy":[26.52554,17.02977,9.85565,21.67083]},
-    {"t":0.42936, "x":6.8286, "y":1.64924, "heading":2.19629, "vx":-1.38185, "vy":0.59253, "omega":-0.51352, "ax":-3.21703, "ay":1.37919, "alpha":-1.19014, "fx":[-40.09072,-44.94819,-47.07254,-42.99505], "fy":[26.57617,17.15043,9.8439,21.50055]},
-    {"t":0.46513, "x":6.7771, "y":1.67133, "heading":2.17791, "vx":-1.49696, "vy":0.64188, "omega":-0.5561, "ax":-3.2167, "ay":1.37897, "alpha":-1.18866, "fx":[-40.04821,-44.89223,-47.06845,-43.07978], "fy":[26.62922,17.28217,9.8344,21.31279]},
-    {"t":0.50091, "x":6.72148, "y":1.69517, "heading":2.15802, "vx":-1.61205, "vy":0.69122, "omega":-0.59863, "ax":-3.21633, "ay":1.3787, "alpha":-1.18714, "fx":[-40.00332,-44.83077,-47.06295,-43.17109], "fy":[26.6841,17.42495,9.82784,21.10713]},
-    {"t":0.53669, "x":6.66174, "y":1.72079, "heading":2.1366, "vx":-1.72713, "vy":0.74055, "omega":-0.64111, "ax":-3.21589, "ay":1.37838, "alpha":-1.18564, "fx":[-39.95625,-44.76355,-47.05568,-43.26863], "fy":[26.74015,17.57867,9.82498,20.88317]},
-    {"t":0.57247, "x":6.59789, "y":1.74817, "heading":2.11366, "vx":-1.84219, "vy":0.78987, "omega":-0.68353, "ax":-3.21536, "ay":1.37802, "alpha":-1.1842, "fx":[-39.90717,-44.69025,-47.04627,-43.37192], "fy":[26.79663,17.74323,9.82656,20.64049]},
-    {"t":0.60825, "x":6.52992, "y":1.77731, "heading":2.0892, "vx":-1.95724, "vy":0.83917, "omega":-0.7259, "ax":-3.21473, "ay":1.37758, "alpha":-1.1829, "fx":[-39.85623,-44.61049,-47.03424,-43.48034], "fy":[26.8527,17.91845,9.83332,20.37871]},
-    {"t":0.64403, "x":6.45783, "y":1.80822, "heading":2.06323, "vx":-2.07226, "vy":0.88846, "omega":-0.76822, "ax":-3.21396, "ay":1.37706, "alpha":-1.18179, "fx":[-39.80347,-44.52378,-47.01899,-43.59305], "fy":[26.90742,18.1041,9.84596,20.09744]},
-    {"t":0.67981, "x":6.38163, "y":1.84089, "heading":2.03574, "vx":-2.18725, "vy":0.93773, "omega":-0.81051, "ax":-3.213, "ay":1.37644, "alpha":-1.18099, "fx":[-39.74883,-44.42951,-46.99975,-43.70895], "fy":[26.95971,18.29985,9.8651,19.79628]},
-    {"t":0.71559, "x":6.30131, "y":1.87532, "heading":2.00674, "vx":-2.30221, "vy":0.98698, "omega":-0.85276, "ax":-3.21178, "ay":1.37568, "alpha":-1.18058, "fx":[-39.69194,-44.32679,-46.97551,-43.8265], "fy":[27.00833,18.50527,9.89124,19.47477]},
-    {"t":0.75137, "x":6.21689, "y":1.91151, "heading":1.97623, "vx":-2.41713, "vy":1.0362, "omega":-0.895, "ax":-3.2102, "ay":1.37474, "alpha":-1.1807, "fx":[-39.632,-44.21438,-46.94478,-43.94352], "fy":[27.05179,18.71969,9.9246,19.13231]},
-    {"t":0.78715, "x":6.12835, "y":1.94947, "heading":1.94421, "vx":-2.53199, "vy":1.08539, "omega":-0.93725, "ax":-3.20809, "ay":1.37354, "alpha":-1.18153, "fx":[-39.56729,-44.09033,-46.90532,-44.05676], "fy":[27.08825,18.94213,9.96492,18.76792]},
-    {"t":0.82293, "x":6.0357, "y":1.98918, "heading":1.91067, "vx":-2.64677, "vy":1.13453, "omega":-0.97952, "ax":-3.20516, "ay":1.37196, "alpha":-1.18331, "fx":[-39.49437,-43.95138,-46.85344,-44.16097], "fy":[27.11533,19.17104,10.01099,18.37985]},
-    {"t":0.85871, "x":5.93895, "y":2.03065, "heading":1.87563, "vx":-2.76145, "vy":1.18362, "omega":-1.02186, "ax":-3.20087, "ay":1.36975, "alpha":-1.18649, "fx":[-39.40597,-43.79147,-46.78242,-44.24681], "fy":[27.12952,19.40363,10.05955,17.96442]},
-    {"t":0.89449, "x":5.8381, "y":2.07388, "heading":1.83907, "vx":-2.87598, "vy":1.23263, "omega":-1.06431, "ax":-3.19407, "ay":1.3664, "alpha":-1.19193, "fx":[-39.28549,-43.59781,-46.67828,-44.29513], "fy":[27.12502,19.63434,10.10232,17.51303]},
-    {"t":0.93027, "x":5.73315, "y":2.11886, "heading":1.80098, "vx":-2.99026, "vy":1.28152, "omega":-1.10696, "ax":-3.1818, "ay":1.36056, "alpha":-1.20194, "fx":[-39.08806,-43.33745,-46.50547,-44.25777], "fy":[27.08958,19.84946,10.11573,17.00176]},
-    {"t":0.96605, "x":5.62412, "y":2.16558, "heading":1.76138, "vx":-3.1041, "vy":1.3302, "omega":-1.14996, "ax":-3.1533, "ay":1.34731, "alpha":-1.22519, "fx":[-38.64694,-42.89164,-46.13689,-43.96215], "fy":[26.98489,20.00125,10.01028,16.33933]},
-    {"t":1.00183, "x":5.51104, "y":2.21404, "heading":1.72023, "vx":-3.21693, "vy":1.37841, "omega":-1.1938, "ax":-3.01623, "ay":1.2846, "alpha":-1.33997, "fx":[-36.51767,-41.25951,-44.49323,-41.90641], "fy":[26.49866,19.69498,9.01479,14.71364]},
-    {"t":1.03761, "x":5.39401, "y":2.26418, "heading":1.67752, "vx":-3.32485, "vy":1.42437, "omega":-1.24175, "ax":3.02045, "ay":-1.29842, "alpha":0.96586, "fx":[37.95041,41.05616,43.6823,41.71769], "fy":[-24.08183,-19.49802,-11.61026,-15.48454]},
-    {"t":1.07339, "x":5.27698, "y":2.31431, "heading":1.63309, "vx":-3.21678, "vy":1.37791, "omega":-1.20719, "ax":3.15676, "ay":-1.35264, "alpha":1.11091, "fx":[39.19593,42.50431,45.82833,44.29711], "fy":[-26.10139,-20.73387,-11.32007,-15.47007]},
-    {"t":1.10917, "x":5.16391, "y":2.36275, "heading":1.5899, "vx":-3.10383, "vy":1.32952, "omega":-1.16744, "ax":3.18388, "ay":-1.36332, "alpha":1.14507, "fx":[39.4596,42.66887,46.22256,44.95127], "fy":[-26.48576,-21.21531,-11.38601,-15.12]},
-    {"t":1.14495, "x":5.05489, "y":2.40944, "heading":1.54813, "vx":-2.98991, "vy":1.28074, "omega":-1.12647, "ax":3.19536, "ay":-1.36784, "alpha":1.16314, "fx":[39.59666,42.65828,46.36596,45.30622], "fy":[-26.61502,-21.57875,-11.51526,-14.74402]},
-    {"t":1.18073, "x":4.94996, "y":2.45439, "heading":1.50782, "vx":-2.87558, "vy":1.2318, "omega":-1.08485, "ax":3.20163, "ay":-1.37034, "alpha":1.17546, "fx":[39.69837,42.59303,46.42468,45.55226], "fy":[-26.64901,-21.89374,-11.66932,-14.37695]},
-    {"t":1.21651, "x":4.84912, "y":2.49759, "heading":1.46901, "vx":-2.76103, "vy":1.18276, "omega":-1.04279, "ax":3.20554, "ay":-1.37194, "alpha":1.18484, "fx":[39.78819,42.50589,46.44431,45.74257], "fy":[-26.63332,-22.17924,-11.83567,-14.02804]},
-    {"t":1.25229, "x":4.75238, "y":2.53903, "heading":1.4317, "vx":-2.64634, "vy":1.13368, "omega":-1.0004, "ax":3.20818, "ay":-1.37308, "alpha":1.19236, "fx":[39.87429,42.40956,46.44263,45.89828], "fy":[-26.58668,-22.44266,-12.00836,-13.70048]},
-    {"t":1.28807, "x":4.65975, "y":2.57871, "heading":1.3959, "vx":-2.53155, "vy":1.08455, "omega":-0.95774, "ax":3.21007, "ay":-1.37395, "alpha":1.19853, "fx":[39.9598,42.31001,46.42812,46.0297], "fy":[-26.51874,-22.68756,-12.18387,-13.39531]},
-    {"t":1.32385, "x":4.57123, "y":2.61664, "heading":1.36163, "vx":-2.41669, "vy":1.03539, "omega":-0.91486, "ax":3.21148, "ay":-1.37465, "alpha":1.20363, "fx":[40.04584,42.21046,46.40543,46.14269], "fy":[-26.43536,-22.91595,-12.35978,-13.1126]},
-    {"t":1.35962, "x":4.48682, "y":2.6528, "heading":1.3289, "vx":-2.30179, "vy":0.98621, "omega":-0.87179, "ax":3.21257, "ay":-1.37524, "alpha":1.20781, "fx":[40.13263,42.11277,46.37736,46.24094], "fy":[-26.34056,-23.12913,-12.53427,-12.8519]},
-    {"t":1.3954, "x":4.40652, "y":2.68721, "heading":1.29771, "vx":-2.18684, "vy":0.937, "omega":-0.82858, "ax":3.21344, "ay":-1.37575, "alpha":1.21121, "fx":[40.21992,42.01812,46.34579,46.32698], "fy":[-26.23737,-23.32802,-12.70593,-12.61248]},
-    {"t":1.43118, "x":4.33033, "y":2.71985, "heading":1.26806, "vx":-2.07187, "vy":0.88778, "omega":-0.78524, "ax":3.21414, "ay":-1.37621, "alpha":1.21394, "fx":[40.30724,41.92727,46.31199,46.4027], "fy":[-26.12824,-23.51336,-12.87356,-12.3934]},
-    {"t":1.46696, "x":4.25826, "y":2.75074, "heading":1.23997, "vx":-1.95687, "vy":0.83854, "omega":-0.74181, "ax":3.21473, "ay":-1.37662, "alpha":1.21607, "fx":[40.39402,41.84072,46.27693,46.46951], "fy":[-26.01525,-23.68576,-13.03619,-12.19366]},
-    {"t":1.50274, "x":4.1903, "y":2.77986, "heading":1.21343, "vx":-1.84185, "vy":0.78928, "omega":-0.69829, "ax":3.21523, "ay":-1.37699, "alpha":1.21768, "fx":[40.47963,41.75878,46.24137,46.52859], "fy":[-25.9002,-23.84576,-13.19299,-12.01217]},
-    {"t":1.53852, "x":4.12646, "y":2.80722, "heading":1.18844, "vx":-1.72681, "vy":0.74001, "omega":-0.65473, "ax":3.21566, "ay":-1.37733, "alpha":1.21887, "fx":[40.56346,41.68168,46.20588,46.58088], "fy":[-25.78471,-23.99386,-13.34322,-11.84781]},
-    {"t":1.5743, "x":4.06673, "y":2.83281, "heading":1.16501, "vx":-1.61175, "vy":0.69073, "omega":-0.61112, "ax":3.21604, "ay":-1.37764, "alpha":1.21968, "fx":[40.64488,41.60955,46.17096,46.62717], "fy":[-25.67022,-24.1305,-13.48629,-11.6995]},
-    {"t":1.61008, "x":4.01112, "y":2.85665, "heading":1.14315, "vx":-1.49668, "vy":0.64144, "omega":-0.56748, "ax":3.21638, "ay":-1.37793, "alpha":1.22019, "fx":[40.72333,41.54245,46.13703,46.66814], "fy":[-25.55806,-24.25614,-13.62163,-11.56615]},
-    {"t":1.64586, "x":3.95963, "y":2.87871, "heading":1.12285, "vx":-1.3816, "vy":0.59214, "omega":-0.52382, "ax":3.21668, "ay":-1.37819, "alpha":1.22047, "fx":[40.79828,41.48042,46.10442,46.70439], "fy":[-25.44941,-24.37116,-13.74881,-11.44673]},
-    {"t":1.68164, "x":3.91225, "y":2.89902, "heading":1.1041, "vx":-1.26651, "vy":0.54283, "omega":-0.48015, "ax":3.21696, "ay":-1.37842, "alpha":1.22056, "fx":[40.86922,41.42345,46.07346,46.7364], "fy":[-25.34538,-24.47597,-13.86741,-11.34024]},
-    {"t":1.71742, "x":3.869, "y":2.91756, "heading":1.08692, "vx":-1.15141, "vy":0.49351, "omega":-0.43648, "ax":3.21721, "ay":-1.37864, "alpha":1.22051, "fx":[40.93572,41.37152,46.04439,46.76461], "fy":[-25.24695,-24.5709,-13.97709,-11.24577]},
-    {"t":1.7532, "x":3.82986, "y":2.93433, "heading":1.07131, "vx":-1.0363, "vy":0.44418, "omega":-0.39281, "ax":3.21744, "ay":-1.37883, "alpha":1.22038, "fx":[40.99738,41.32458,46.01745,46.78941], "fy":[-25.155,-24.65631,-14.07755,-11.16246]},
-    {"t":1.78898, "x":3.79484, "y":2.94934, "heading":1.05725, "vx":-0.92118, "vy":0.39485, "omega":-0.34915, "ax":3.21765, "ay":-1.37901, "alpha":1.22019, "fx":[41.05386,41.28258,45.99284,46.81112], "fy":[-25.07033,-24.7325,-14.16854,-11.0895]},
-    {"t":1.82476, "x":3.76394, "y":2.96259, "heading":1.04476, "vx":-0.80605, "vy":0.34551, "omega":-0.30549, "ax":3.21785, "ay":-1.37916, "alpha":1.21999, "fx":[41.10484,41.24547,45.97074,46.83002], "fy":[-24.99364,-24.79975,-14.24984,-11.02619]},
-    {"t":1.86054, "x":3.73716, "y":2.97407, "heading":1.03383, "vx":-0.69092, "vy":0.29616, "omega":-0.26184, "ax":3.21803, "ay":-1.3793, "alpha":1.21981, "fx":[41.15006,41.21319,45.95129,46.84637], "fy":[-24.92557,-24.85832,-14.32127,-10.97189]},
-    {"t":1.89632, "x":3.7145, "y":2.98378, "heading":1.02446, "vx":-0.57578, "vy":0.24681, "omega":-0.21819, "ax":3.2182, "ay":-1.37943, "alpha":1.21966, "fx":[41.18931,41.18568,45.93463,46.86036], "fy":[-24.86665,-24.90844,-14.38266,-10.92603]},
-    {"t":1.9321, "x":3.69596, "y":2.99173, "heading":1.01665, "vx":-0.46063, "vy":0.19746, "omega":-0.17455, "ax":3.21835, "ay":-1.37954, "alpha":1.21957, "fx":[41.22239,41.16289,45.92088,46.87217], "fy":[-24.81734,-24.9503,-14.43389,-10.88815]},
-    {"t":1.96788, "x":3.68154, "y":2.99791, "heading":1.01041, "vx":-0.34548, "vy":0.1481, "omega":-0.13092, "ax":3.21849, "ay":-1.37963, "alpha":1.21956, "fx":[41.24915,41.14479,45.91012,46.88195], "fy":[-24.77802,-24.98406,-14.47486,-10.85783]},
-    {"t":2.00366, "x":3.67124, "y":3.00233, "heading":1.00572, "vx":-0.23033, "vy":0.09873, "omega":-0.08728, "ax":3.21862, "ay":-1.37971, "alpha":1.21964, "fx":[41.26947,41.13133,45.90242,46.88982], "fy":[-24.749,-25.00985,-14.50547,-10.83476]},
-    {"t":2.03944, "x":3.66506, "y":3.00498, "heading":1.0026, "vx":-0.11517, "vy":0.04937, "omega":-0.04364, "ax":3.21874, "ay":-1.37978, "alpha":1.21981, "fx":[41.28326,41.12249,45.89786,46.89586], "fy":[-24.73053,-25.02779,-14.52567,-10.81871]},
-    {"t":2.07522, "x":3.663, "y":3.00586, "heading":1.00104, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.87023, "ay":-2.06751, "alpha":-0.16069, "fx":[-38.7583,-39.7416,-39.35153,-38.37825], "fy":[-28.55723,-27.17332,-27.73786,-29.06862]},
-    {"t":2.11883, "x":3.66027, "y":3.00389, "heading":1.00104, "vx":-0.12518, "vy":-0.09017, "omega":-0.00701, "ax":-2.86958, "ay":-2.06814, "alpha":-0.16058, "fx":[-38.74934,-39.73237,-39.34275,-38.36977], "fy":[-28.56558,-27.18287,-27.74657,-29.07621]},
-    {"t":2.16244, "x":3.65208, "y":2.99799, "heading":1.00073, "vx":-0.25033, "vy":-0.18037, "omega":-0.01401, "ax":-2.86886, "ay":-2.06884, "alpha":-0.16045, "fx":[-38.73924,-39.7221,-39.33327,-38.36051], "fy":[-28.57508,-27.19353,-27.75589,-29.08444]},
-    {"t":2.20606, "x":3.63843, "y":2.98816, "heading":1.00012, "vx":-0.37545, "vy":-0.2706, "omega":-0.02101, "ax":-2.86806, "ay":-2.0696, "alpha":-0.16031, "fx":[-38.72785,-39.71065,-39.32294,-38.35033], "fy":[-28.58587,-27.20544,-27.76597,-29.09346]},
-    {"t":2.24967, "x":3.61933, "y":2.97439, "heading":0.99921, "vx":-0.50054, "vy":-0.36086, "omega":-0.028, "ax":-2.86718, "ay":-2.07046, "alpha":-0.16015, "fx":[-38.715,-39.69781,-39.31158,-38.33907], "fy":[-28.59811,-27.2188,-27.77697,-29.1034]},
-    {"t":2.29328, "x":3.59477, "y":2.95668, "heading":0.99799, "vx":-0.62558, "vy":-0.45116, "omega":-0.03499, "ax":-2.86618, "ay":-2.07142, "alpha":-0.15998, "fx":[-38.70045,-39.68336,-39.29896,-38.32649], "fy":[-28.612,-27.23385,-27.78913,-29.11446]},
-    {"t":2.3369, "x":3.56476, "y":2.93504, "heading":0.99646, "vx":-0.75059, "vy":-0.5415, "omega":-0.04196, "ax":-2.86506, "ay":-2.0725, "alpha":-0.15978, "fx":[-38.68391,-39.667,-39.2848,-38.31234], "fy":[-28.62782,-27.2509,-27.80272,-29.12688]},
-    {"t":2.38051, "x":3.5293, "y":2.90945, "heading":0.99463, "vx":-0.87554, "vy":-0.63189, "omega":-0.04893, "ax":-2.86377, "ay":-2.07373, "alpha":-0.15956, "fx":[-38.66503,-39.64835,-39.26874,-38.29626], "fy":[-28.64589,-27.27033,-27.8181,-29.14096]},
-    {"t":2.42412, "x":3.48839, "y":2.87992, "heading":0.9925, "vx":-1.00044, "vy":-0.72233, "omega":-0.05589, "ax":-2.8623, "ay":-2.07514, "alpha":-0.1593, "fx":[-38.64332,-39.6269,-39.2503,-38.2778], "fy":[-28.66665,-27.29264,-27.83573,-29.15712]},
-    {"t":2.46774, "x":3.44204, "y":2.84644, "heading":0.99006, "vx":-1.12527, "vy":-0.81283, "omega":-0.06284, "ax":-2.8606, "ay":-2.07678, "alpha":-0.159, "fx":[-38.61815,-39.60201,-39.22886,-38.25636], "fy":[-28.69067,-27.31849,-27.85622,-29.17586]},
-    {"t":2.51135, "x":3.39024, "y":2.80902, "heading":0.98732, "vx":-1.25003, "vy":-0.90341, "omega":-0.06977, "ax":-2.85859, "ay":-2.0787, "alpha":-0.15865, "fx":[-38.58869,-39.57281,-39.20359,-38.23114], "fy":[-28.71871,-27.34877,-27.88038,-29.19789]},
-    {"t":2.55496, "x":3.333, "y":2.76764, "heading":0.98427, "vx":-1.37471, "vy":-0.99407, "omega":-0.07669, "ax":-2.8562, "ay":-2.08098, "alpha":-0.15823, "fx":[-38.55376,-39.53806,-39.17332,-38.20104], "fy":[-28.75183,-27.3847,-27.90934,-29.2242]},
-    {"t":2.59857, "x":3.27033, "y":2.7223, "heading":0.98093, "vx":-1.49927, "vy":-1.08483, "omega":-0.08359, "ax":-2.85331, "ay":-2.08374, "alpha":-0.15773, "fx":[-38.5117,-39.49605,-39.13641,-38.16445], "fy":[-28.79153,-27.428,-27.9447,-29.25615]},
-    {"t":2.64219, "x":3.20223, "y":2.67301, "heading":0.97728, "vx":-1.62372, "vy":-1.1757, "omega":-0.09047, "ax":-2.84973, "ay":-2.08715, "alpha":-0.15711, "fx":[-38.46004,-39.44422,-39.09042,-38.11905], "fy":[-28.84001,-27.48123,-27.98881,-29.29579]},
-    {"t":2.6858, "x":3.12871, "y":2.61975, "heading":0.97334, "vx":-1.748, "vy":-1.26673, "omega":-0.09732, "ax":-2.84519, "ay":-2.09146, "alpha":-0.15632, "fx":[-38.39503,-39.37865,-39.0316,-38.06125], "fy":[-28.90064,-27.54824,-28.04525,-29.34622]},
-    {"t":2.72941, "x":3.04976, "y":2.56251, "heading":0.96909, "vx":-1.87209, "vy":-1.35795, "omega":-0.10414, "ax":-2.83924, "ay":-2.09708, "alpha":-0.1553, "fx":[-38.31056,-39.293,-38.95392,-37.98524], "fy":[-28.97882,-27.63529,-28.11978,-29.41242]},
-    {"t":2.77303, "x":2.96542, "y":2.5013, "heading":0.96455, "vx":-1.99592, "vy":-1.44941, "omega":-0.11091, "ax":-2.83111, "ay":-2.10472, "alpha":-0.1539, "fx":[-38.19607,-39.17629,-38.84685,-37.88095], "fy":[-29.08385,-27.75306,-28.22233,-29.50301]},
-    {"t":2.81664, "x":2.87568, "y":2.43608, "heading":0.95971, "vx":-2.11939, "vy":-1.5412, "omega":-0.11763, "ax":-2.81932, "ay":-2.11571, "alpha":-0.15189, "fx":[-38.03153,-39.00762,-38.69046,-37.72928], "fy":[-29.23314,-27.92163,-28.37151,-29.6341]},
-    {"t":2.86025, "x":2.78056, "y":2.36685, "heading":0.95458, "vx":-2.24235, "vy":-1.63347, "omega":-0.12425, "ax":-2.80073, "ay":-2.13286, "alpha":-0.14873, "fx":[-37.77387,-38.742,-38.44167,-37.4891], "fy":[-29.46352,-28.18349,-28.60682,-29.83989]},
-    {"t":2.90387, "x":2.6801, "y":2.29358, "heading":0.94917, "vx":-2.3645, "vy":-1.72649, "omega":-0.13074, "ax":-2.76701, "ay":-2.16334, "alpha":-0.14307, "fx":[-37.31029,-38.26116,-37.98722,-37.05256], "fy":[-29.86885,-28.64704,-29.02932,-30.20775]},
-    {"t":2.94748, "x":2.57435, "y":2.21623, "heading":0.94346, "vx":-2.48518, "vy":-1.82084, "omega":-0.13698, "ax":-2.68721, "ay":-2.23248, "alpha":-0.12983, "fx":[-36.2229,-37.12355,-36.90295,-36.01859], "fy":[-30.77869,-29.69415,-29.9972,-31.04651]},
-    {"t":2.99109, "x":2.4634, "y":2.13469, "heading":0.93749, "vx":-2.60237, "vy":-1.91821, "omega":-0.14264, "ax":-2.27323, "ay":-2.53307, "alpha":-0.06343, "fx":[-30.69378,-31.20535,-31.1703,-30.66523], "fy":[-34.66508,-34.20896,-34.27729,-34.7264]},
-    {"t":3.03471, "x":2.34774, "y":2.04862, "heading":0.93127, "vx":-2.70152, "vy":-2.02868, "omega":-0.14541, "ax":3.13325, "ay":1.32981, "alpha":0.25193, "fx":[42.38604,43.33765,42.90072,41.92186], "fy":[18.53827,16.35411,17.69976,19.79099]},
-    {"t":3.07832, "x":2.2329, "y":1.96141, "heading":0.92493, "vx":-2.56487, "vy":-1.97069, "omega":-0.13442, "ax":2.98697, "ay":1.81136, "alpha":0.19407, "fx":[40.317,41.3638,40.9708,39.93256], "fy":[25.17017,23.43314,24.16494,25.82614]},
-    {"t":3.12193, "x":2.12388, "y":1.87718, "heading":0.91906, "vx":-2.43459, "vy":-1.89169, "omega":-0.12595, "ax":2.94809, "ay":1.9088, "alpha":0.1817, "fx":[39.77322,40.81774,40.45409,39.42288], "fy":[26.4981,24.87022,25.48558,27.04423]},
-    {"t":3.16555, "x":2.0205, "y":1.7965, "heading":0.91357, "vx":-2.30602, "vy":-1.80844, "omega":-0.11803, "ax":2.93043, "ay":1.9505, "alpha":0.17636, "fx":[39.52533,40.56726,40.22037,39.1937], "fy":[27.06738,25.48648,26.04979,27.56439]},
-    {"t":3.20916, "x":1.92272, "y":1.71948, "heading":0.90842, "vx":-2.17821, "vy":-1.72337, "omega":-0.11034, "ax":2.92036, "ay":1.97365, "alpha":0.1734, "fx":[39.38289,40.42342,40.08816,39.06407], "fy":[27.38476,25.82936,26.36141,27.85212]},
-    {"t":3.25277, "x":1.8305, "y":1.6462, "heading":0.90361, "vx":-2.05085, "vy":-1.63729, "omega":-0.10278, "ax":2.91386, "ay":1.98836, "alpha":0.17151, "fx":[39.28997,40.3299,40.00363,38.981], "fy":[27.58782,26.04809,26.55833,28.03436]},
-    {"t":3.29639, "x":1.74382, "y":1.57668, "heading":0.89913, "vx":-1.92376, "vy":-1.55058, "omega":-0.0953, "ax":2.90931, "ay":1.99854, "alpha":0.17021, "fx":[39.2243,40.26408,39.9452,38.9234], "fy":[27.7293,26.19998,26.69361,28.15989]},
-    {"t":3.34, "x":1.66269, "y":1.51095, "heading":0.89497, "vx":-1.79688, "vy":-1.46341, "omega":-0.08787, "ax":2.90595, "ay":2.00601, "alpha":0.16925, "fx":[39.17527,40.21517,39.90257,38.88122], "fy":[27.83376,26.31175,26.79204,28.25148]},
-    {"t":3.38361, "x":1.58709, "y":1.44904, "heading":0.89114, "vx":-1.67014, "vy":-1.37592, "omega":-0.08049, "ax":2.90337, "ay":2.01171, "alpha":0.16853, "fx":[39.13716,40.17733,39.87018,38.84906], "fy":[27.91418,26.39752,26.86672,28.32117]},
-    {"t":3.42723, "x":1.51701, "y":1.39094, "heading":0.88763, "vx":-1.54352, "vy":-1.28819, "omega":-0.07314, "ax":2.90133, "ay":2.01621, "alpha":0.16795, "fx":[39.10666,40.14716,39.8448,38.82377], "fy":[27.97808,26.46547,26.92526,28.37592]},
-    {"t":3.47084, "x":1.45245, "y":1.33668, "heading":0.88444, "vx":-1.41698, "vy":-1.20025, "omega":-0.06582, "ax":2.89967, "ay":2.01986, "alpha":0.16749, "fx":[39.08166,40.12253,39.8244,38.80336], "fy":[28.03009,26.52065,26.97234,28.42006]},
-    {"t":3.51445, "x":1.39341, "y":1.28625, "heading":0.88157, "vx":-1.29052, "vy":-1.11216, "omega":-0.05851, "ax":2.89829, "ay":2.02287, "alpha":0.16711, "fx":[39.06082,40.10204,39.80763,38.78656], "fy":[28.07325,26.56634,27.01104,28.45638]},
-    {"t":3.55806, "x":1.33988, "y":1.23967, "heading":0.87902, "vx":-1.16411, "vy":-1.02394, "omega":-0.05122, "ax":2.89713, "ay":2.0254, "alpha":0.16679, "fx":[39.04319,40.08474,39.79359,38.77248], "fy":[28.10961,26.6048,27.04342,28.48682]},
-    {"t":3.60168, "x":1.29186, "y":1.19694, "heading":0.87678, "vx":-1.03776, "vy":-0.9356, "omega":-0.04395, "ax":2.89614, "ay":2.02755, "alpha":0.16651, "fx":[39.02811,40.06995,39.78164,38.76048], "fy":[28.14062,26.63759,27.07097,28.51271]},
-    {"t":3.64529, "x":1.24936, "y":1.15806, "heading":0.87487, "vx":-0.91145, "vy":-0.84718, "omega":-0.03669, "ax":2.89529, "ay":2.02941, "alpha":0.16628, "fx":[39.0151,40.05718,39.77131,38.75012], "fy":[28.16733,26.66585,27.09473,28.53504]},
-    {"t":3.6889, "x":1.21236, "y":1.12305, "heading":0.87327, "vx":-0.78518, "vy":-0.75867, "omega":-0.02943, "ax":2.89454, "ay":2.03103, "alpha":0.16607, "fx":[39.00381,40.04606,39.76226,38.74106], "fy":[28.19052,26.69043,27.1155,28.55452]},
-    {"t":3.73252, "x":1.18087, "y":1.09189, "heading":0.87198, "vx":-0.65894, "vy":-0.67009, "omega":-0.02219, "ax":2.89389, "ay":2.03245, "alpha":0.16589, "fx":[38.99396,40.03632,39.75421,38.73304], "fy":[28.21078,26.71196,27.13387,28.5717]},
-    {"t":3.77613, "x":1.15488, "y":1.0646, "heading":0.87102, "vx":-0.53273, "vy":-0.58145, "omega":-0.01496, "ax":2.89331, "ay":2.03371, "alpha":0.16574, "fx":[38.98534,40.02775,39.74696,38.72586], "fy":[28.22856,26.73094,27.1503,28.58701]},
-    {"t":3.81974, "x":1.1344, "y":1.04117, "heading":0.87036, "vx":-0.40654, "vy":-0.49275, "omega":-0.00773, "ax":2.89279, "ay":2.03483, "alpha":0.16559, "fx":[38.97779,40.02016,39.74036,38.71937], "fy":[28.24421,26.74776,27.16515,28.60078]},
-    {"t":3.86336, "x":1.11942, "y":1.02162, "heading":0.87003, "vx":-0.28038, "vy":-0.404, "omega":-0.00051, "ax":2.89232, "ay":2.03584, "alpha":0.16547, "fx":[38.97118,40.01344,39.73427,38.71344], "fy":[28.25803,26.76272,27.1787,28.61328]},
-    {"t":3.90697, "x":1.10995, "y":1.00593, "heading":0.87, "vx":-0.15423, "vy":-0.31521, "omega":0.00671, "ax":2.8919, "ay":2.03674, "alpha":0.16535, "fx":[38.96541,40.00748,39.7286,38.70797], "fy":[28.27023,26.77606,27.19119,28.6247]},
-    {"t":3.95058, "x":1.10597, "y":0.99412, "heading":0.8703, "vx":-0.02811, "vy":-0.22639, "omega":0.01392, "ax":2.89152, "ay":2.03757, "alpha":0.16525, "fx":[38.96037,40.00217,39.72325,38.70288], "fy":[28.28101,26.788,27.20281,28.63523]},
-    {"t":3.9942, "x":1.10749, "y":0.98619, "heading":0.8709, "vx":0.098, "vy":-0.13752, "omega":0.02113, "ax":2.8912, "ay":2.03809, "alpha":0.15792, "fx":[38.97352,39.96884,39.7022,38.72642], "fy":[28.26249,26.83725,27.23297,28.60288]},
-    {"t":4.03752, "x":1.11445, "y":0.98214, "heading":0.87182, "vx":0.22326, "vy":-0.04922, "omega":0.02797, "ax":2.89082, "ay":2.03835, "alpha":0.15787, "fx":[38.96894,39.96385,39.69658,38.72114], "fy":[28.26503,26.84075,27.23743,28.60644]},
-    {"t":4.08085, "x":1.12684, "y":0.98192, "heading":0.87303, "vx":0.34851, "vy":0.03909, "omega":0.03481, "ax":2.89041, "ay":2.03863, "alpha":0.15782, "fx":[38.96404,39.95843,39.69026,38.71525], "fy":[28.26764,26.8445,27.24251,28.61045]},
-    {"t":4.12417, "x":1.14465, "y":0.98553, "heading":0.87454, "vx":0.47373, "vy":0.12741, "omega":0.04165, "ax":2.88995, "ay":2.03895, "alpha":0.15775, "fx":[38.95873,39.9525,39.68319,38.70868], "fy":[28.27037,26.84856,27.24828,28.61497]},
-    {"t":4.1675, "x":1.16789, "y":0.99296, "heading":0.87634, "vx":0.59894, "vy":0.21575, "omega":0.04848, "ax":2.88944, "ay":2.0393, "alpha":0.15769, "fx":[38.95292,39.94594,39.67524,38.70133], "fy":[28.27329,26.85301,27.25479,28.62006]},
-    {"t":4.21082, "x":1.19655, "y":1.00423, "heading":0.87844, "vx":0.72412, "vy":0.3041, "omega":0.05531, "ax":2.88887, "ay":2.03969, "alpha":0.15761, "fx":[38.94647,39.93865,39.66632,38.69309], "fy":[28.27648,26.85793,27.26215,28.62578]},
-    {"t":4.25415, "x":1.23063, "y":1.01932, "heading":0.88084, "vx":0.84928, "vy":0.39247, "omega":0.06214, "ax":2.88823, "ay":2.04012, "alpha":0.15752, "fx":[38.93925,39.93045,39.65625,38.6838], "fy":[28.28004,26.86346,27.27045,28.63223]},
-    {"t":4.29747, "x":1.27014, "y":1.03823, "heading":0.88353, "vx":0.97442, "vy":0.48086, "omega":0.06897, "ax":2.88751, "ay":2.04062, "alpha":0.15743, "fx":[38.93104,39.92115,39.64486,38.67329], "fy":[28.28409,26.86974,27.27982,28.63951]},
-    {"t":4.34079, "x":1.31506, "y":1.06098, "heading":0.88652, "vx":1.09952, "vy":0.56927, "omega":0.07579, "ax":2.88668, "ay":2.04119, "alpha":0.15732, "fx":[38.9216,39.91048,39.6319,38.66132], "fy":[28.28879,26.87696,27.29045,28.64777]},
-    {"t":4.38412, "x":1.36541, "y":1.08756, "heading":0.8898, "vx":1.22458, "vy":0.6577, "omega":0.0826, "ax":2.88573, "ay":2.04184, "alpha":0.15719, "fx":[38.91061,39.89812,39.61704,38.64757], "fy":[28.29435,26.88537,27.30256,28.65721]},
-    {"t":4.42744, "x":1.42117, "y":1.11797, "heading":0.89338, "vx":1.3496, "vy":0.74616, "omega":0.08941, "ax":2.88461, "ay":2.0426, "alpha":0.15704, "fx":[38.89763,39.88361,39.59985,38.63162], "fy":[28.30106,26.89531,27.31646,28.66808]},
-    {"t":4.47077, "x":1.48235, "y":1.15222, "heading":0.89726, "vx":1.47458, "vy":0.83466, "omega":0.09622, "ax":2.8833, "ay":2.0435, "alpha":0.15687, "fx":[38.88205,39.86634,39.57976,38.61292], "fy":[28.3093,26.90723,27.33254,28.68071]},
-    {"t":4.51409, "x":1.54894, "y":1.1903, "heading":0.90143, "vx":1.5995, "vy":0.92319, "omega":0.10301, "ax":2.88172, "ay":2.04457, "alpha":0.15666, "fx":[38.86305,39.84545,39.55594,38.59067], "fy":[28.31961,26.92175,27.35139,28.69561]},
-    {"t":4.55742, "x":1.62094, "y":1.23221, "heading":0.90589, "vx":1.72435, "vy":1.01177, "omega":0.1098, "ax":2.87979, "ay":2.04589, "alpha":0.15641, "fx":[38.83941,39.81971,39.52721,38.5637], "fy":[28.33275,26.93979,27.37384,28.71347]},
-    {"t":4.60074, "x":1.69835, "y":1.27797, "heading":0.91065, "vx":1.84911, "vy":1.10041, "omega":0.11658, "ax":2.87738, "ay":2.04753, "alpha":0.15609, "fx":[38.80933,39.78726,39.4918,38.5303], "fy":[28.34989,26.96269,27.40111,28.73535]},
-    {"t":4.64407, "x":1.78117, "y":1.32756, "heading":0.9157, "vx":1.97378, "vy":1.18912, "omega":0.12334, "ax":2.87427, "ay":2.04963, "alpha":0.1557, "fx":[38.76995,39.7452,39.44692,38.48773], "fy":[28.37286,26.99259,27.43515,28.76289]},
-    {"t":4.68739, "x":1.86938, "y":1.38101, "heading":0.92104, "vx":2.0983, "vy":1.27792, "omega":0.13009, "ax":2.87014, "ay":2.05242, "alpha":0.15517, "fx":[38.71652,39.68867,39.38794,38.43147], "fy":[28.40465,27.03298,27.47917,28.79885]},
-    {"t":4.73072, "x":1.96298, "y":1.4383, "heading":0.92668, "vx":2.22265, "vy":1.36684, "omega":0.13681, "ax":2.86434, "ay":2.05632, "alpha":0.15444, "fx":[38.64048,39.60894,39.30647,38.35334], "fy":[28.45069,27.09017,27.53892,28.84817]},
-    {"t":4.77404, "x":2.06196, "y":1.49945, "heading":0.9326, "vx":2.34675, "vy":1.45593, "omega":0.1435, "ax":2.85565, "ay":2.06215, "alpha":0.15337, "fx":[38.52471,39.48852,39.18576,38.23694], "fy":[28.52173,27.17664,27.62585,28.92063]},
-    {"t":4.81737, "x":2.16632, "y":1.56446, "heading":0.93882, "vx":2.47047, "vy":1.54527, "omega":0.15014, "ax":2.84114, "ay":2.07178, "alpha":0.1516, "fx":[38.32922,39.28657,38.98662,38.04403], "fy":[28.64255,27.3212,27.76642,29.03893]},
-    {"t":4.86069, "x":2.27602, "y":1.63335, "heading":0.94532, "vx":2.59356, "vy":1.63503, "omega":0.15671, "ax":2.81208, "ay":2.09078, "alpha":0.14813, "fx":[37.93398,38.88017,38.59107,37.65959], "fy":[28.8861,27.60871,28.03866,29.26987]},
-    {"t":4.90402, "x":2.39102, "y":1.70615, "heading":0.95211, "vx":2.71539, "vy":1.72562, "omega":0.16313, "ax":2.72458, "ay":2.14573, "alpha":0.13779, "fx":[36.73867,37.65139,37.40473,36.50711], "fy":[29.59953,28.44481,28.81713,29.93286]},
-    {"t":4.94734, "x":2.51122, "y":1.78293, "heading":0.95918, "vx":2.83343, "vy":1.81858, "omega":0.1691, "ax":-1.43393, "ay":1.9915, "alpha":-0.33918, "fx":[-17.85593,-19.20235,-21.11804,-19.87426], "fy":[27.91503,28.10094,26.31646,26.06687]},
-    {"t":4.99067, "x":2.63263, "y":1.86359, "heading":0.96651, "vx":2.77131, "vy":1.90486, "omega":0.1544, "ax":-2.94554, "ay":-1.83028, "alpha":-0.17785, "fx":[-39.79114,-40.75222,-40.37074,-39.41457], "fy":[-25.33672,-23.79398,-24.50549,-25.9878]},
-    {"t":5.03399, "x":2.74994, "y":1.9444, "heading":0.9732, "vx":2.6437, "vy":1.82556, "omega":0.1467, "ax":-2.92388, "ay":-1.93111, "alpha":-0.16823, "fx":[-39.49439,-40.45558,-40.07691,-39.12332], "fy":[-26.71006,-25.2452,-25.87445,-27.28273]},
-    {"t":5.07732, "x":2.86173, "y":2.02168, "heading":0.97955, "vx":2.51702, "vy":1.7419, "omega":0.13941, "ax":-2.91599, "ay":-1.96487, "alpha":-0.16503, "fx":[-39.38913,-40.34771,-39.9671,-39.01653], "fy":[-27.16507,-25.72918,-26.33747,-27.71835]},
-    {"t":5.12064, "x":2.96804, "y":2.0953, "heading":0.98559, "vx":2.39068, "vy":1.65677, "omega":0.13226, "ax":-2.91191, "ay":-1.98177, "alpha":-0.16345, "fx":[-39.33673,-40.2927,-39.90865,-38.96061], "fy":[-27.39001,-25.97018,-26.57204,-27.93778]},
-    {"t":5.16397, "x":3.06888, "y":2.16522, "heading":0.99132, "vx":2.26453, "vy":1.57091, "omega":0.12518, "ax":-2.90943, "ay":-1.99192, "alpha":-0.16252, "fx":[-39.30617,-40.25969,-39.8717,-38.92587], "fy":[-27.52301,-26.11396,-26.71485,-28.07046]},
-    {"t":5.20729, "x":3.16426, "y":2.23141, "heading":0.99675, "vx":2.13848, "vy":1.48461, "omega":0.11814, "ax":-2.90776, "ay":-1.99868, "alpha":-0.1619, "fx":[-39.28667,-40.23786,-39.84581,-38.90199], "fy":[-27.61015,-26.20917,-26.81157,-28.15963]},
-    {"t":5.25062, "x":3.25418, "y":2.29385, "heading":1.00187, "vx":2.0125, "vy":1.39802, "omega":0.11112, "ax":-2.90655, "ay":-2.00352, "alpha":-0.16147, "fx":[-39.27348,-40.22248,-39.82641,-38.88445], "fy":[-27.6712,-26.27669,-26.88183,-28.22385]},
-    {"t":5.29394, "x":3.33865, "y":2.35254, "heading":1.00668, "vx":1.88657, "vy":1.31122, "omega":0.10413, "ax":-2.90565, "ay":-2.00714, "alpha":-0.16115, "fx":[-39.26421,-40.21112,-39.81117,-38.87095], "fy":[-27.71605,-26.32695,-26.93545,-28.27241]},
-    {"t":5.33726, "x":3.41766, "y":2.40747, "heading":1.01119, "vx":1.76069, "vy":1.22426, "omega":0.09715, "ax":-2.90494, "ay":-2.00996, "alpha":-0.1609, "fx":[-39.2575,-40.20245,-39.79878,-38.8602], "fy":[-27.75019,-26.36576,-26.97787,-28.3105]},
-    {"t":5.38059, "x":3.49121, "y":2.45862, "heading":1.0154, "vx":1.63483, "vy":1.13718, "omega":0.09018, "ax":-2.90437, "ay":-2.01222, "alpha":-0.1607, "fx":[-39.25252,-40.19562,-39.78846,-38.8514], "fy":[-27.77692,-26.39659,-27.01237,-28.34119]},
-    {"t":5.42391, "x":3.55931, "y":2.506, "heading":1.01931, "vx":1.509, "vy":1.05, "omega":0.08321, "ax":-2.9039, "ay":-2.01406, "alpha":-0.16054, "fx":[-39.24876,-40.19013,-39.7797,-38.84406], "fy":[-27.79834,-26.42165,-27.04104,-28.36648]},
-    {"t":5.46724, "x":3.62196, "y":2.5496, "heading":1.02291, "vx":1.38319, "vy":0.96274, "omega":0.07626, "ax":-2.90352, "ay":-2.0156, "alpha":-0.1604, "fx":[-39.24586,-40.18563,-39.77217,-38.83784], "fy":[-27.81585,-26.44241,-27.06526,-28.38769]},
-    {"t":5.51056, "x":3.67917, "y":2.58942, "heading":1.02622, "vx":1.25739, "vy":0.87541, "omega":0.06931, "ax":-2.90319, "ay":-2.0169, "alpha":-0.16029, "fx":[-39.24358,-40.18187,-39.76561,-38.8325], "fy":[-27.83041,-26.4599,-27.08599,-28.40572]},
-    {"t":5.55389, "x":3.73092, "y":2.62545, "heading":1.02922, "vx":1.13161, "vy":0.78803, "omega":0.06236, "ax":-2.9029, "ay":-2.01801, "alpha":-0.1602, "fx":[-39.24174,-40.17868,-39.75988,-38.82787], "fy":[-27.84273,-26.47482,-27.10391,-28.42124]},
-    {"t":5.59721, "x":3.77722, "y":2.6577, "heading":1.03192, "vx":1.00585, "vy":0.7006, "omega":0.05542, "ax":-2.90266, "ay":-2.01898, "alpha":-0.16011, "fx":[-39.24022,-40.17592,-39.75484,-38.82384], "fy":[-27.85331,-26.48773,-27.11955,-28.43472]},
-    {"t":5.64054, "x":3.81807, "y":2.68616, "heading":1.03432, "vx":0.88009, "vy":0.61313, "omega":0.04849, "ax":-2.90244, "ay":-2.01983, "alpha":-0.16004, "fx":[-39.23891,-40.17352,-39.7504,-38.8203], "fy":[-27.86253,-26.49901,-27.13326,-28.44653]},
-    {"t":5.68386, "x":3.85348, "y":2.71083, "heading":1.03642, "vx":0.75434, "vy":0.52562, "omega":0.04155, "ax":-2.90225, "ay":-2.02057, "alpha":-0.15998, "fx":[-39.23773,-40.17139,-39.7465,-38.81718], "fy":[-27.87068,-26.50898,-27.14534,-28.45693]},
-    {"t":5.72719, "x":3.88344, "y":2.7317, "heading":1.03822, "vx":0.6286, "vy":0.43808, "omega":0.03462, "ax":-2.90208, "ay":-2.02124, "alpha":-0.15992, "fx":[-39.23665,-40.16948,-39.74307,-38.81442], "fy":[-27.87799,-26.51786,-27.15601,-28.46616]},
-    {"t":5.77051, "x":3.90795, "y":2.74879, "heading":1.03972, "vx":0.50287, "vy":0.35051, "omega":0.02769, "ax":-2.90193, "ay":-2.02183, "alpha":-0.15987, "fx":[-39.23559,-40.16774,-39.74007,-38.81199], "fy":[-27.88464,-26.52585,-27.16545,-28.47436]},
-    {"t":5.81384, "x":3.92701, "y":2.76207, "heading":1.04092, "vx":0.37714, "vy":0.26292, "omega":0.02077, "ax":-2.9018, "ay":-2.02236, "alpha":-0.15982, "fx":[-39.23454,-40.16614,-39.73747,-38.80984], "fy":[-27.89078,-26.53309,-27.1738,-28.48169]},
-    {"t":5.85716, "x":3.94063, "y":2.77157, "heading":1.04182, "vx":0.25142, "vy":0.1753, "omega":0.01384, "ax":-2.90167, "ay":-2.02285, "alpha":-0.15978, "fx":[-39.23346,-40.16465,-39.73524,-38.80795], "fy":[-27.89651,-26.5397,-27.18118,-28.48825]},
-    {"t":5.90049, "x":3.9488, "y":2.77726, "heading":1.04242, "vx":0.12571, "vy":0.08766, "omega":0.00692, "ax":-2.90156, "ay":-2.02328, "alpha":-0.15975, "fx":[-39.23232,-40.16324,-39.73335,-38.80629], "fy":[-27.90194,-26.54579,-27.18767,-28.49414]},
-    {"t":5.94381, "x":3.95152, "y":2.77916, "heading":1.04272, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.00808, "ay":-1.86111, "alpha":-0.16883, "fx":[-40.72739,-41.60415,-41.14124,-40.26022], "fy":[-25.67046,-24.22545,-25.00647,-26.40006]},
-    {"t":5.98721, "x":3.94869, "y":2.77741, "heading":1.04272, "vx":-0.13055, "vy":-0.08077, "omega":-0.00733, "ax":-3.00794, "ay":-1.86103, "alpha":-0.16882, "fx":[-40.72553,-41.60225,-41.13943,-40.25845], "fy":[-25.66926,-24.2244,-25.00539,-26.39885]},
-    {"t":6.03061, "x":3.94019, "y":2.77215, "heading":1.0424, "vx":-0.2611, "vy":-0.16155, "omega":-0.01465, "ax":-3.0078, "ay":-1.86094, "alpha":-0.16882, "fx":[-40.72325,-41.60009,-41.13765,-40.25659], "fy":[-25.6683,-24.22334,-25.00386,-26.3974]},
-    {"t":6.07402, "x":3.92602, "y":2.76339, "heading":1.04177, "vx":-0.39164, "vy":-0.24231, "omega":-0.02198, "ax":-3.00763, "ay":-1.86084, "alpha":-0.16881, "fx":[-40.72054,-41.59766,-41.13587,-40.25459], "fy":[-25.66755,-24.22226,-25.00187,-26.3957]},
-    {"t":6.11742, "x":3.90619, "y":2.75112, "heading":1.04081, "vx":-0.52218, "vy":-0.32307, "omega":-0.02931, "ax":-3.00745, "ay":-1.86072, "alpha":-0.1688, "fx":[-40.71735,-41.59491,-41.13405,-40.25243], "fy":[-25.66699,-24.22113,-24.99939,-26.39372]},
-    {"t":6.16082, "x":3.8807, "y":2.73534, "heading":1.03954, "vx":-0.65271, "vy":-0.40383, "omega":-0.03663, "ax":-3.00725, "ay":-1.8606, "alpha":-0.16879, "fx":[-40.71364,-41.5918,-41.13215,-40.25007], "fy":[-25.66659,-24.21994,-24.99641,-26.39144]},
-    {"t":6.20422, "x":3.84954, "y":2.71606, "heading":1.03795, "vx":-0.78322, "vy":-0.48458, "omega":-0.04396, "ax":-3.00702, "ay":-1.86046, "alpha":-0.16878, "fx":[-40.70936,-41.58827,-41.13011,-40.24744], "fy":[-25.66632,-24.21865,-24.99288,-26.38882]},
-    {"t":6.24762, "x":3.81271, "y":2.69328, "heading":1.03604, "vx":-0.91373, "vy":-0.56533, "omega":-0.05128, "ax":-3.00676, "ay":-1.8603, "alpha":-0.16877, "fx":[-40.70444,-41.58425,-41.12787,-40.2445], "fy":[-25.66613,-24.21723,-24.98877,-26.3858]},
-    {"t":6.29102, "x":3.77022, "y":2.66699, "heading":1.03382, "vx":-1.04423, "vy":-0.64607, "omega":-0.05861, "ax":-3.00646, "ay":-1.86011, "alpha":-0.16876, "fx":[-40.69878,-41.57965,-41.12534,-40.24114], "fy":[-25.66596,-24.21561,-24.98402,-26.38234]},
-    {"t":6.33442, "x":3.72207, "y":2.6372, "heading":1.03127, "vx":-1.17471, "vy":-0.7268, "omega":-0.06593, "ax":-3.00612, "ay":-1.8599, "alpha":-0.16874, "fx":[-40.69227,-41.57433,-41.1224,-40.23727], "fy":[-25.66573,-24.21375,-24.97858,-26.37835]},
-    {"t":6.37782, "x":3.66825, "y":2.6039, "heading":1.02841, "vx":-1.30518, "vy":-0.80752, "omega":-0.07326, "ax":-3.00572, "ay":-1.85965, "alpha":-0.16873, "fx":[-40.68476,-41.56815,-41.1189,-40.23273], "fy":[-25.66535,-24.21155,-24.97234,-26.37373]},
-    {"t":6.42122, "x":3.60878, "y":2.56711, "heading":1.02523, "vx":-1.43563, "vy":-0.88823, "omega":-0.08058, "ax":-3.00525, "ay":-1.85936, "alpha":-0.16871, "fx":[-40.67602,-41.56089,-41.11464,-40.22732], "fy":[-25.66466,-24.20888,-24.96518,-26.36834]},
-    {"t":6.46463, "x":3.54364, "y":2.5268, "heading":1.02174, "vx":-1.56606, "vy":-0.96893, "omega":-0.0879, "ax":-3.00468, "ay":-1.85901, "alpha":-0.16869, "fx":[-40.66576,-41.55222,-41.10931,-40.22075], "fy":[-25.66349,-24.20558,-24.95694,-26.36199]},
-    {"t":6.50803, "x":3.47284, "y":2.483, "heading":1.01792, "vx":-1.69647, "vy":-1.04962, "omega":-0.09522, "ax":-3.00399, "ay":-1.85858, "alpha":-0.16866, "fx":[-40.65354,-41.54171,-41.1025,-40.21262], "fy":[-25.66155,-24.20141,-24.94735,-26.35439]},
-    {"t":6.55143, "x":3.39638, "y":2.4357, "heading":1.01379, "vx":-1.82685, "vy":-1.13028, "omega":-0.10254, "ax":-3.00312, "ay":-1.85805, "alpha":-0.16863, "fx":[-40.63871,-41.52871,-41.09358,-40.2023], "fy":[-25.65842,-24.19597,-24.93605,-26.34513]},
-    {"t":6.59483, "x":3.31427, "y":2.38489, "heading":1.00934, "vx":-1.95719, "vy":-1.21092, "omega":-0.10986, "ax":-3.00201, "ay":-1.85736, "alpha":-0.1686, "fx":[-40.62025,-41.51216,-41.08155,-40.18883], "fy":[-25.65345,-24.1887,-24.92243,-26.33355]},
-    {"t":6.63823, "x":3.22649, "y":2.33059, "heading":1.00457, "vx":-2.08748, "vy":-1.29153, "omega":-0.11718, "ax":-3.00053, "ay":-1.85644, "alpha":-0.16856, "fx":[-40.59645,-41.49034,-41.06478,-40.17057], "fy":[-25.64555,-24.17859,-24.90551,-26.31858]},
-    {"t":6.68163, "x":3.13307, "y":2.27278, "heading":0.99948, "vx":-2.2177, "vy":-1.3721, "omega":-0.1245, "ax":-2.99846, "ay":-1.85516, "alpha":-0.16851, "fx":[-40.56424,-41.46015,-41.04029,-40.1446], "fy":[-25.63276,-24.16386,-24.88351,-26.29828]},
-    {"t":6.72503, "x":3.03399, "y":2.21149, "heading":0.99408, "vx":-2.34784, "vy":-1.45262, "omega":-0.13181, "ax":-2.99535, "ay":-1.85324, "alpha":-0.16845, "fx":[-40.51749,-41.4154,-41.00217,-40.10507], "fy":[-25.6112,-24.14092,-24.85285,-26.26878]},
-    {"t":6.76843, "x":2.92927, "y":2.14669, "heading":0.98836, "vx":-2.47784, "vy":-1.53305, "omega":-0.13912, "ax":-2.99018, "ay":-1.85004, "alpha":-0.16837, "fx":[-40.44189,-41.34167,-40.93666,-40.03833], "fy":[-25.57178,-24.10143,-24.80522,-26.22111]},
-    {"t":6.81183, "x":2.81892, "y":2.07842, "heading":0.98232, "vx":-2.60762, "vy":-1.61335, "omega":-0.14643, "ax":-2.97986, "ay":-1.84365, "alpha":-0.16827, "fx":[-40.29472,-41.19594,-40.80261,-39.90356], "fy":[-25.48743,-24.02032,-24.71574,-26.12851]},
-    {"t":6.85524, "x":2.70294, "y":2.00666, "heading":0.97597, "vx":-2.73695, "vy":-1.69336, "omega":-0.15373, "ax":-2.94911, "ay":-1.82463, "alpha":-0.16812, "fx":[-39.86404,-40.76496,-40.39596,-39.49825], "fy":[-25.22399,-23.77328,-24.46091,-25.85835]},
-    {"t":6.89864, "x":2.58137, "y":1.93145, "heading":0.96929, "vx":-2.86494, "vy":-1.77255, "omega":-0.16103, "ax":0.0, "ay":0.0, "alpha":-0.00063, "fx":[0.00251,-0.00042,-0.00242,0.0005], "fy":[0.00048,0.00249,-0.00044,-0.00245]},
-    {"t":6.94204, "x":2.45703, "y":1.85451, "heading":0.96231, "vx":-2.86494, "vy":-1.77255, "omega":-0.16105, "ax":2.94911, "ay":1.82463, "alpha":0.1681, "fx":[39.85513,40.7615,40.40424,39.50236], "fy":[25.23835,23.77852,24.44699,25.85268]},
-    {"t":6.98544, "x":2.33547, "y":1.7793, "heading":0.95532, "vx":-2.73695, "vy":-1.69336, "omega":-0.15376, "ax":2.97986, "ay":1.84365, "alpha":0.16826, "fx":[40.2767,41.18946,40.81932,39.91135], "fy":[25.51617,24.03073,24.68789,26.11722]},
-    {"t":7.02884, "x":2.21949, "y":1.70754, "heading":0.94864, "vx":-2.60762, "vy":-1.61335, "omega":-0.14646, "ax":2.99018, "ay":1.85004, "alpha":0.16837, "fx":[40.41521,41.33228,40.9614,40.04966], "fy":[25.61419,24.11683,24.76411,26.20441]},
-    {"t":7.07224, "x":2.10913, "y":1.63927, "heading":0.94229, "vx":-2.47784, "vy":-1.53305, "omega":-0.13915, "ax":2.99535, "ay":1.85324, "alpha":0.16846, "fx":[40.48257,41.40323,41.03454,40.11978], "fy":[25.66661,24.1611,24.79914,26.2469]},
-    {"t":7.11564, "x":2.00441, "y":1.57448, "heading":0.93625, "vx":-2.34784, "vy":-1.45262, "omega":-0.13184, "ax":2.99846, "ay":1.85516, "alpha":0.16852, "fx":[40.52153,41.44535,41.07988,40.16252], "fy":[25.70049,24.18857,24.81787,26.27148]},
-    {"t":7.15904, "x":1.90533, "y":1.51318, "heading":0.93053, "vx":-2.2177, "vy":-1.3721, "omega":-0.12452, "ax":3.00053, "ay":1.85644, "alpha":0.16858, "fx":[40.54637,41.47304,41.11119,40.19153], "fy":[25.72491,24.2076,24.8286,26.28713]},
-    {"t":7.20245, "x":1.81191, "y":1.45537, "heading":0.92512, "vx":-2.08748, "vy":-1.29153, "omega":-0.11721, "ax":3.00201, "ay":1.85736, "alpha":0.16862, "fx":[40.56323,41.49251,41.13439,40.21265], "fy":[25.74375,24.22177,24.8349,26.29771]},
-    {"t":7.24585, "x":1.72414, "y":1.40107, "heading":0.92003, "vx":-1.95719, "vy":-1.21092, "omega":-0.10989, "ax":3.00312, "ay":1.85805, "alpha":0.16866, "fx":[40.5752,41.50684,41.15244,40.22882], "fy":[25.75898,24.23286,24.83858,26.30516]},
-    {"t":7.28925, "x":1.64202, "y":1.35026, "heading":0.91526, "vx":-1.82685, "vy":-1.13028, "omega":-0.10257, "ax":3.00399, "ay":1.85858, "alpha":0.16869, "fx":[40.58396,41.51777,41.16698,40.24165], "fy":[25.77168,24.24186,24.8406,26.31055]},
-    {"t":7.33265, "x":1.56556, "y":1.30296, "heading":0.91081, "vx":-1.69647, "vy":-1.04961, "omega":-0.09525, "ax":3.00468, "ay":1.85901, "alpha":0.16872, "fx":[40.59055,41.52635,41.179,40.25213], "fy":[25.7825,24.24936,24.84157,26.31456]},
-    {"t":7.37605, "x":1.49477, "y":1.25916, "heading":0.90668, "vx":-1.56606, "vy":-0.96893, "omega":-0.08792, "ax":3.00525, "ay":1.85936, "alpha":0.16874, "fx":[40.59562,41.53323,41.18914,40.26086], "fy":[25.79187,24.25573,24.84187,26.31759]},
-    {"t":7.41945, "x":1.42963, "y":1.21885, "heading":0.90286, "vx":-1.43563, "vy":-0.88823, "omega":-0.0806, "ax":3.00572, "ay":1.85965, "alpha":0.16876, "fx":[40.5996,41.53886,41.19782,40.26826], "fy":[25.80006,24.26123,24.84174,26.31992]},
-    {"t":7.46285, "x":1.37015, "y":1.18206, "heading":0.89937, "vx":-1.30518, "vy":-0.80752, "omega":-0.07328, "ax":3.00612, "ay":1.8599, "alpha":0.16878, "fx":[40.60279,41.54354,41.20533,40.27462], "fy":[25.80727,24.26602,24.84136,26.32175]},
-    {"t":7.50625, "x":1.31633, "y":1.14876, "heading":0.89618, "vx":-1.17471, "vy":-0.7268, "omega":-0.06595, "ax":3.00646, "ay":1.86011, "alpha":0.1688, "fx":[40.6054,41.5475,41.21187,40.28013], "fy":[25.81364,24.27021,24.84085,26.32323]},
-    {"t":7.54965, "x":1.26818, "y":1.11897, "heading":0.89332, "vx":-1.04423, "vy":-0.64607, "omega":-0.05863, "ax":3.00676, "ay":1.8603, "alpha":0.16881, "fx":[40.6076,41.5509,41.21761,40.28494], "fy":[25.81926,24.27391,24.8403,26.32444]},
-    {"t":7.59306, "x":1.22569, "y":1.09268, "heading":0.89078, "vx":-0.91373, "vy":-0.56533, "omega":-0.0513, "ax":3.00702, "ay":1.86046, "alpha":0.16882, "fx":[40.6095,41.55386,41.22265,40.28917], "fy":[25.82422,24.27717,24.83979,26.32548]},
-    {"t":7.63646, "x":1.18887, "y":1.0699, "heading":0.88855, "vx":-0.78322, "vy":-0.48458, "omega":-0.04397, "ax":3.00725, "ay":1.8606, "alpha":0.16883, "fx":[40.61119,41.55647,41.22709,40.2929], "fy":[25.82858,24.28005,24.83935,26.3264]},
-    {"t":7.67986, "x":1.15771, "y":1.05062, "heading":0.88664, "vx":-0.65271, "vy":-0.40383, "omega":-0.03664, "ax":3.00745, "ay":1.86072, "alpha":0.16884, "fx":[40.61274,41.55881,41.23099,40.2962], "fy":[25.83237,24.28257,24.83904,26.32725]},
-    {"t":7.72326, "x":1.13221, "y":1.03484, "heading":0.88505, "vx":-0.52218, "vy":-0.32307, "omega":-0.02932, "ax":3.00763, "ay":1.86084, "alpha":0.16885, "fx":[40.6142,41.56094,41.23441,40.2991], "fy":[25.83565,24.28478,24.83888,26.32807]},
-    {"t":7.76666, "x":1.11238, "y":1.02257, "heading":0.88378, "vx":-0.39164, "vy":-0.24231, "omega":-0.02199, "ax":3.0078, "ay":1.86094, "alpha":0.16886, "fx":[40.61562,41.5629,41.23738,40.30167], "fy":[25.83843,24.28669,24.83889,26.32888]},
-    {"t":7.81006, "x":1.09822, "y":1.01381, "heading":0.88283, "vx":-0.2611, "vy":-0.16155, "omega":-0.01466, "ax":3.00794, "ay":1.86103, "alpha":0.16887, "fx":[40.61703,41.56472,41.23996,40.30393], "fy":[25.84075,24.28832,24.8391,26.32972]},
-    {"t":7.85346, "x":1.08972, "y":1.00855, "heading":0.88219, "vx":-0.13055, "vy":-0.08077, "omega":-0.00733, "ax":3.00808, "ay":1.86111, "alpha":0.16888, "fx":[40.61847,41.56645,41.24216,40.3059], "fy":[25.84261,24.28969,24.83953,26.3306]},
-    {"t":7.89686, "x":1.08688, "y":1.0068, "heading":0.88187, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.05492, "ay":2.85624, "alpha":-0.88813, "fx":[31.82003,24.67623,23.27118,32.08398], "fy":[36.125,41.33598,42.131,35.87611]},
-    {"t":7.93397, "x":1.0883, "y":1.00876, "heading":0.88187, "vx":0.07626, "vy":0.10599, "omega":-0.03296, "ax":2.05483, "ay":2.85612, "alpha":-0.88807, "fx":[31.81855,24.67547,23.27043,32.08214], "fy":[36.12377,41.33425,42.1288,35.87464]},
-    {"t":7.97108, "x":1.09254, "y":1.01466, "heading":0.88065, "vx":0.15251, "vy":0.21198, "omega":-0.06591, "ax":2.05474, "ay":2.85598, "alpha":-0.88798, "fx":[31.82115,24.67993,23.26505,32.07534], "fy":[36.11874,41.32923,42.12896,35.87733]},
-    {"t":8.00819, "x":1.09962, "y":1.0245, "heading":0.8782, "vx":0.22876, "vy":0.31797, "omega":-0.09887, "ax":2.05464, "ay":2.85584, "alpha":-0.88787, "fx":[31.82781,24.68962,23.25506,32.06356], "fy":[36.1099,41.3209,42.13144,35.88418]},
-    {"t":8.0453, "x":1.10952, "y":1.03826, "heading":0.87453, "vx":0.30501, "vy":0.42395, "omega":-0.13182, "ax":2.05453, "ay":2.85568, "alpha":-0.88774, "fx":[31.83846,24.70454,23.24052,32.04671], "fy":[36.09726,41.30921,42.13616,35.89521]},
-    {"t":8.08241, "x":1.12226, "y":1.05596, "heading":0.86964, "vx":0.38125, "vy":0.52992, "omega":-0.16476, "ax":2.05441, "ay":2.85551, "alpha":-0.88757, "fx":[31.85304,24.7247,23.22151,32.0247], "fy":[36.08082,41.29412,42.14305,35.91045]},
-    {"t":8.11952, "x":1.13782, "y":1.07759, "heading":0.86353, "vx":0.45749, "vy":0.63588, "omega":-0.1977, "ax":2.05429, "ay":2.85532, "alpha":-0.88736, "fx":[31.87147,24.75014,23.19813,31.99738], "fy":[36.06062,41.27556,42.152,35.92994]},
-    {"t":8.15663, "x":1.15621, "y":1.10316, "heading":0.85619, "vx":0.53372, "vy":0.74184, "omega":-0.23063, "ax":2.05415, "ay":2.85511, "alpha":-0.88712, "fx":[31.89364,24.78088,23.17054,31.96461], "fy":[36.03667,41.25347,42.16287,35.95374]},
-    {"t":8.19374, "x":1.17743, "y":1.13265, "heading":0.84763, "vx":0.60995, "vy":0.8478, "omega":-0.26355, "ax":2.054, "ay":2.85488, "alpha":-0.88682, "fx":[31.91942,24.81696,23.13889,31.92619], "fy":[36.00901,41.22773,42.17548,35.98192]},
-    {"t":8.23085, "x":1.20148, "y":1.16608, "heading":0.83785, "vx":0.68617, "vy":0.95374, "omega":-0.29646, "ax":2.05383, "ay":2.85462, "alpha":-0.88647, "fx":[31.94866,24.85841,23.10339,31.88189], "fy":[35.97767,41.19825,42.18966,36.01454]},
-    {"t":8.26796, "x":1.22836, "y":1.20344, "heading":0.82685, "vx":0.76239, "vy":1.05967, "omega":-0.32935, "ax":2.05365, "ay":2.85433, "alpha":-0.88605, "fx":[31.98117,24.90527,23.06424,31.83146], "fy":[35.94271,41.1649,42.20517,36.05169]},
-    {"t":8.30507, "x":1.25806, "y":1.24473, "heading":0.81463, "vx":0.8386, "vy":1.16559, "omega":-0.36223, "ax":2.05343, "ay":2.85401, "alpha":-0.88556, "fx":[32.01674,24.95757,23.02171,31.77458], "fy":[35.90415,41.12753,42.22175,36.09343]},
-    {"t":8.34218, "x":1.2906, "y":1.28995, "heading":0.80119, "vx":0.9148, "vy":1.27151, "omega":-0.3951, "ax":2.05319, "ay":2.85364, "alpha":-0.88497, "fx":[32.05511,25.01533,22.97606,31.71091], "fy":[35.86206,41.08595,42.23908,36.13981]},
-    {"t":8.37929, "x":1.32596, "y":1.3391, "heading":0.78653, "vx":0.99099, "vy":1.3774, "omega":-0.42794, "ax":2.05291, "ay":2.85322, "alpha":-0.88428, "fx":[32.09597,25.07857,22.92761,31.64006], "fy":[35.81646,41.03994,42.25678,36.19086]},
-    {"t":8.4164, "x":1.36415, "y":1.39218, "heading":0.77065, "vx":1.06718, "vy":1.48328, "omega":-0.46075, "ax":2.05259, "ay":2.85274, "alpha":-0.88348, "fx":[32.13895,25.14727,22.87666,31.56157], "fy":[35.76739,40.98926,42.27442,36.2466]},
-    {"t":8.45351, "x":1.40516, "y":1.44918, "heading":0.75355, "vx":1.14335, "vy":1.58915, "omega":-0.49354, "ax":2.0522, "ay":2.85217, "alpha":-0.88253, "fx":[32.18358,25.22139,22.82355,31.47491], "fy":[35.71486,40.93356,42.29145,36.30695]},
-    {"t":8.49062, "x":1.449, "y":1.51012, "heading":0.73523, "vx":1.2195, "vy":1.69499, "omega":-0.52629, "ax":2.05174, "ay":2.8515, "alpha":-0.88143, "fx":[32.22931,25.30083,22.76863,31.37942], "fy":[35.65882,40.87243,42.30719,36.37175]},
-    {"t":8.52772, "x":1.49567, "y":1.57498, "heading":0.7157, "vx":1.29564, "vy":1.80081, "omega":-0.559, "ax":2.05117, "ay":2.85069, "alpha":-0.88014, "fx":[32.27538,25.38542,22.71223,31.27431], "fy":[35.59917,40.80532,42.32078,36.44071]},
-    {"t":8.56483, "x":1.54517, "y":1.64377, "heading":0.69496, "vx":1.37176, "vy":1.9066, "omega":-0.59166, "ax":2.05046, "ay":2.84969, "alpha":-0.87863, "fx":[32.32081,25.47485,22.65465,31.15856], "fy":[35.53569,40.73145,42.33103,36.51329]},
-    {"t":8.60194, "x":1.59748, "y":1.71649, "heading":0.673, "vx":1.44785, "vy":2.01235, "omega":-0.62426, "ax":2.04956, "ay":2.84842, "alpha":-0.87685, "fx":[32.36421,25.56861,22.5961,31.03075], "fy":[35.46792,40.6497,42.33629,36.58856]},
-    {"t":8.63905, "x":1.65262, "y":1.79313, "heading":0.64984, "vx":1.52391, "vy":2.11805, "omega":-0.6568, "ax":2.04837, "ay":2.84676, "alpha":-0.87472, "fx":[32.40349,25.66582,22.53663,30.88884], "fy":[35.395,40.55833,42.33402,36.66489]},
-    {"t":8.67616, "x":1.71058, "y":1.87369, "heading":0.62546, "vx":1.59992, "vy":2.22369, "omega":-0.68926, "ax":2.04673, "ay":2.8445, "alpha":-0.87212, "fx":[32.43526,25.76487,22.4759,30.72952], "fy":[35.31526,40.45439,42.32009,36.73937]},
-    {"t":8.71327, "x":1.77137, "y":1.95816, "heading":0.59988, "vx":1.67588, "vy":2.32925, "omega":-0.72163, "ax":2.04434, "ay":2.84123, "alpha":-0.86883, "fx":[32.45344,25.86271,22.41276,30.5469], "fy":[35.22527,40.33245,42.28698,36.80629]},
-    {"t":8.75038, "x":1.83496, "y":2.04656, "heading":0.57311, "vx":1.75174, "vy":2.43468, "omega":-0.75387, "ax":2.04058, "ay":2.83607, "alpha":-0.86438, "fx":[32.44549,25.95269,22.34394,30.32877], "fy":[35.11727,40.18098,42.21898,36.85333]},
-    {"t":8.78749, "x":1.90138, "y":2.13886, "heading":0.54513, "vx":1.82746, "vy":2.53993, "omega":-0.78595, "ax":2.03378, "ay":2.82679, "alpha":-0.85756, "fx":[32.37959,26.01745,22.25972,30.04412], "fy":[34.97042,39.97022,42.07605,36.8484]},
-    {"t":8.8246, "x":1.97059, "y":2.23506, "heading":0.51596, "vx":1.90294, "vy":2.64483, "omega":-0.81777, "ax":2.01793, "ay":2.80513, "alpha":-0.84403, "fx":[32.14129,25.99327,22.122,29.58134], "fy":[34.70682,39.59148,41.71351,36.67452]},
-    {"t":8.86171, "x":2.0426, "y":2.33514, "heading":0.48562, "vx":1.97782, "vy":2.74893, "omega":-0.84909, "ax":1.9398, "ay":2.69832, "alpha":-0.78409, "fx":[30.72315,25.30781,21.57836,27.97636], "fy":[33.62238,38.07319,39.86384,35.31286]},
-    {"t":8.89882, "x":2.11733, "y":2.43901, "heading":0.45411, "vx":2.04981, "vy":2.84906, "omega":-0.87819, "ax":-1.94046, "ay":-2.6941, "alpha":0.94772, "fx":[-31.63338,-25.29383,-20.49064,-28.20334], "fy":[-32.86045,-38.20492,-40.49223,-35.08495]},
-    {"t":8.93593, "x":2.19206, "y":2.54288, "heading":0.42152, "vx":1.9778, "vy":2.74908, "omega":-0.84302, "ax":-2.01772, "ay":-2.80313, "alpha":0.89526, "fx":[-32.60602,-26.37956,-21.65892,-29.18192], "fy":[-34.26869,-39.35202,-41.97582,-36.98113]},
-    {"t":8.97304, "x":2.26407, "y":2.64297, "heading":0.39023, "vx":1.90292, "vy":2.64506, "omega":-0.8098, "ax":-2.03362, "ay":-2.82563, "alpha":0.8844, "fx":[-32.85261,-26.72476,-21.8815,-29.23315], "fy":[-34.51685,-39.50887,-42.29058,-37.48606]},
-    {"t":9.01015, "x":2.33328, "y":2.73918, "heading":0.36018, "vx":1.82745, "vy":2.5402, "omega":-0.77698, "ax":-2.04046, "ay":-2.83536, "alpha":0.87956, "fx":[-32.98647,-26.95532,-21.97059,-29.15228], "fy":[-34.59805,-39.52095,-42.42993,-37.78285]},
-    {"t":9.04726, "x":2.39969, "y":2.83149, "heading":0.33135, "vx":1.75173, "vy":2.43498, "omega":-0.74434, "ax":-2.04426, "ay":-2.8408, "alpha":0.87675, "fx":[-33.0785,-27.14359,-22.01903,-29.03043], "fy":[-34.62674,-39.48632,-42.50814,-38.00635]},
-    {"t":9.08437, "x":2.46329, "y":2.9199, "heading":0.30373, "vx":1.67587, "vy":2.32956, "omega":-0.7118, "ax":-2.04667, "ay":-2.84427, "alpha":0.8749, "fx":[-33.14857,-27.3094,-22.05174,-28.89302], "fy":[-34.63413,-39.43216,-42.557,-38.1934]},
-    {"t":9.12148, "x":2.52407, "y":3.00439, "heading":0.27731, "vx":1.59992, "vy":2.22401, "omega":-0.67933, "ax":-2.04833, "ay":-2.84669, "alpha":0.8736, "fx":[-33.20468,-27.46035,-22.07785,-28.75014], "fy":[-34.63203,-39.36908,-42.58904,-38.35808]},
-    {"t":9.15859, "x":2.58204, "y":3.08496, "heading":0.2521, "vx":1.52391, "vy":2.11838, "omega":-0.64691, "ax":-2.04954, "ay":-2.84847, "alpha":0.87266, "fx":[-33.25078,-27.59999,-22.10126,-28.60671], "fy":[-34.62577,-39.30212,-42.6103,-38.5069]},
-    {"t":9.1957, "x":2.63718, "y":3.16161, "heading":0.2281, "vx":1.44785, "vy":2.01267, "omega":-0.61453, "ax":-2.05046, "ay":-2.84983, "alpha":0.87199, "fx":[-33.28914,-27.73014,-22.12375,-28.46548], "fy":[-34.61801,-39.234,-42.62414,-38.64329]},
-    {"t":9.2328, "x":2.68949, "y":3.23434, "heading":0.20529, "vx":1.37176, "vy":1.90692, "omega":-0.58217, "ax":-2.05117, "ay":-2.85091, "alpha":0.87152, "fx":[-33.32126,-27.85189,-22.14614,-28.32813], "fy":[-34.61013,-39.16635,-42.63264,-38.76922]},
-    {"t":9.26991, "x":2.73899, "y":3.30314, "heading":0.18369, "vx":1.29564, "vy":1.80112, "omega":-0.54983, "ax":-2.05174, "ay":-2.85179, "alpha":0.87121, "fx":[-33.34817,-27.96588,-22.16874,-28.19578], "fy":[-34.60289,-39.10018,-42.63717,-38.88589]},
-    {"t":9.30702, "x":2.78565, "y":3.36802, "heading":0.16328, "vx":1.2195, "vy":1.69529, "omega":-0.5175, "ax":-2.05221, "ay":-2.85252, "alpha":0.87102, "fx":[-33.37067,-28.07254,-22.19158,-28.0692], "fy":[-34.59667,-39.03619,-42.63871,-38.99412]},
-    {"t":9.34413, "x":2.8295, "y":3.42896, "heading":0.14408, "vx":1.14335, "vy":1.58944, "omega":-0.48518, "ax":-2.0526, "ay":-2.85313, "alpha":0.87093, "fx":[-33.3894,-28.17219,-22.2146,-27.94893], "fy":[-34.59162,-38.97488,-42.63798,-39.09447]},
-    {"t":9.38124, "x":2.87051, "y":3.48598, "heading":0.12608, "vx":1.06718, "vy":1.48356, "omega":-0.45286, "ax":-2.05292, "ay":-2.85365, "alpha":0.87092, "fx":[-33.40489,-28.26503,-22.2376,-27.83538], "fy":[-34.58778,-38.9166,-42.63555,-39.18736]},
-    {"t":9.41835, "x":2.9087, "y":3.53907, "heading":0.10927, "vx":0.99099, "vy":1.37766, "omega":-0.42054, "ax":-2.0532, "ay":-2.8541, "alpha":0.87098, "fx":[-33.41761,-28.35122,-22.2604,-27.72886], "fy":[-34.5851,-38.86161,-42.63188,-39.2731]},
-    {"t":9.45546, "x":2.94406, "y":3.58823, "heading":0.09366, "vx":0.9148, "vy":1.27175, "omega":-0.38822, "ax":-2.05344, "ay":-2.85449, "alpha":0.87107, "fx":[-33.42795,-28.43091,-22.28277,-27.62959], "fy":[-34.58351,-38.81012,-42.62735,-39.35197]},
-    {"t":9.49257, "x":2.9766, "y":3.63346, "heading":0.07926, "vx":0.8386, "vy":1.16582, "omega":-0.35589, "ax":-2.05365, "ay":-2.85483, "alpha":0.8712, "fx":[-33.43626,-28.50419,-22.30447,-27.53773], "fy":[-34.58286,-38.76228,-42.62227,-39.42416]},
-    {"t":9.52968, "x":3.0063, "y":3.67476, "heading":0.06605, "vx":0.76239, "vy":1.05988, "omega":-0.32356, "ax":-2.05384, "ay":-2.85513, "alpha":0.87135, "fx":[-33.44288,-28.57115,-22.32529,-27.45342], "fy":[-34.58304,-38.71823,-42.61691,-39.48987]},
-    {"t":9.56679, "x":3.03318, "y":3.71212, "heading":0.05404, "vx":0.68617, "vy":0.95392, "omega":-0.29123, "ax":-2.05401, "ay":-2.8554, "alpha":0.8715, "fx":[-33.44808,-28.63187,-22.34503,-27.37675], "fy":[-34.58389,-38.67806,-42.61149,-39.54927]},
-    {"t":9.6039, "x":3.05723, "y":3.74555, "heading":0.04324, "vx":0.60995, "vy":0.84796, "omega":-0.25888, "ax":-2.05415, "ay":-2.85564, "alpha":0.87166, "fx":[-33.45211,-28.68642,-22.36348,-27.30777], "fy":[-34.58528,-38.64185,-42.60622,-39.60248]},
-    {"t":9.64101, "x":3.07845, "y":3.77506, "heading":0.03363, "vx":0.53372, "vy":0.74199, "omega":-0.22654, "ax":-2.05429, "ay":-2.85586, "alpha":0.87181, "fx":[-33.45521,-28.73486,-22.38047,-27.24654], "fy":[-34.58708,-38.60968,-42.60127,-39.64964]},
-    {"t":9.67812, "x":3.09684, "y":3.80062, "heading":0.02522, "vx":0.45749, "vy":0.63601, "omega":-0.19419, "ax":-2.05441, "ay":-2.85606, "alpha":0.87194, "fx":[-33.45758,-28.77723,-22.39584,-27.19308], "fy":[-34.58915,-38.58158,-42.59678,-39.69086]},
-    {"t":9.71523, "x":3.1124, "y":3.82226, "heading":0.01802, "vx":0.38125, "vy":0.53002, "omega":-0.16183, "ax":-2.05452, "ay":-2.85624, "alpha":0.87205, "fx":[-33.45939,-28.81358,-22.40945,-27.14742], "fy":[-34.59139,-38.5576,-42.59288,-39.72623]},
-    {"t":9.75234, "x":3.12514, "y":3.83996, "heading":0.01201, "vx":0.30501, "vy":0.42403, "omega":-0.12947, "ax":-2.05463, "ay":-2.8564, "alpha":0.87214, "fx":[-33.46079,-28.84396,-22.42119,-27.10957], "fy":[-34.5937,-38.53779,-42.58967,-39.75584]},
-    {"t":9.78945, "x":3.13504, "y":3.85373, "heading":0.00721, "vx":0.22876, "vy":0.31803, "omega":-0.0971, "ax":-2.05472, "ay":-2.85655, "alpha":0.8722, "fx":[-33.46192,-28.86838,-22.43095,-27.07954], "fy":[-34.59599,-38.52216,-42.58725,-39.77976]},
-    {"t":9.82656, "x":3.14211, "y":3.86357, "heading":0.0036, "vx":0.15251, "vy":0.21203, "omega":-0.06474, "ax":-2.05481, "ay":-2.85669, "alpha":0.87223, "fx":[-33.46287,-28.88689,-22.43866,-27.05733], "fy":[-34.5982,-38.51074,-42.58568,-39.79806]},
-    {"t":9.86367, "x":3.14636, "y":3.86947, "heading":0.0012, "vx":0.07626, "vy":0.10601, "omega":-0.03237, "ax":-2.0549, "ay":-2.85682, "alpha":0.87222, "fx":[-33.46373,-28.89951,-22.44426,-27.04294], "fy":[-34.60027,-38.50355,-42.58502,-39.81078]},
-    {"t":9.90077, "x":3.14777, "y":3.87143, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.98888, "ay":2.90408, "alpha":-0.8559, "fx":[-25.98491,-21.61386,-28.10717,-32.55114], "fy":[40.51082,43.01354,39.08649,35.46115]},
-    {"t":9.9385, "x":3.14636, "y":3.8735, "heading":0.0, "vx":-0.07502, "vy":0.10954, "omega":-0.03229, "ax":-1.9888, "ay":2.90395, "alpha":-0.85592, "fx":[-25.98364,-21.61292,-28.10615,-32.54984], "fy":[40.50886,43.01184,39.08505,35.45957]},
-    {"t":9.97622, "x":3.14211, "y":3.8797, "heading":-0.00122, "vx":-0.15004, "vy":0.21908, "omega":-0.06457, "ax":-1.98871, "ay":2.90382, "alpha":-0.85589, "fx":[-25.99021,-21.6097,-28.09921,-32.54866], "fy":[40.50165,43.01111,39.0877,35.45768]},
-    {"t":10.01394, "x":3.13504, "y":3.89003, "heading":-0.00365, "vx":-0.22506, "vy":0.32862, "omega":-0.09686, "ax":-1.98862, "ay":2.90368, "alpha":-0.85582, "fx":[-26.00462,-21.60422,-28.08633,-32.54755], "fy":[40.48915,43.0113,39.09442,35.45548]},
-    {"t":10.05166, "x":3.12513, "y":3.90449, "heading":-0.00731, "vx":-0.30007, "vy":0.43815, "omega":-0.12914, "ax":-1.98852, "ay":2.90352, "alpha":-0.8557, "fx":[-26.02686,-21.59652,-28.06747,-32.54645], "fy":[40.47133,43.01237,39.10521,35.45301]},
-    {"t":10.08938, "x":3.1124, "y":3.92308, "heading":-0.01218, "vx":-0.37508, "vy":0.54767, "omega":-0.16142, "ax":-1.98841, "ay":2.90335, "alpha":-0.85555, "fx":[-26.05692,-21.58667,-28.04263,-32.54527], "fy":[40.44812,43.01426,39.12002,35.45031]},
-    {"t":10.1271, "x":3.09684, "y":3.94581, "heading":-0.01827, "vx":-0.45008, "vy":0.65719, "omega":-0.19369, "ax":-1.9883, "ay":2.90317, "alpha":-0.85536, "fx":[-26.09479,-21.57473,-28.01177,-32.5439], "fy":[40.41945,43.01689,39.13884,35.44744]},
-    {"t":10.16482, "x":3.07845, "y":3.97266, "heading":-0.02557, "vx":-0.52508, "vy":0.7667, "omega":-0.22595, "ax":-1.98817, "ay":2.90296, "alpha":-0.85514, "fx":[-26.14048,-21.56083,-27.97485,-32.5422], "fy":[40.38526,43.02015,39.16163,35.44449]},
-    {"t":10.20254, "x":3.05722, "y":4.00365, "heading":-0.0341, "vx":-0.60008, "vy":0.8762, "omega":-0.25821, "ax":-1.98803, "ay":2.90274, "alpha":-0.85488, "fx":[-26.19395,-21.54507,-27.93182,-32.53999], "fy":[40.34543,43.02393,39.18834,35.44155]},
-    {"t":10.24026, "x":3.03318, "y":4.03876, "heading":-0.04384, "vx":-0.67507, "vy":0.9857, "omega":-0.29046, "ax":-1.98788, "ay":2.90249, "alpha":-0.85459, "fx":[-26.25519,-21.5276,-27.88264,-32.53707], "fy":[40.29986,43.02809,39.21892,35.43872]},
-    {"t":10.27798, "x":3.0063, "y":4.07801, "heading":-0.05479, "vx":-0.75006, "vy":1.09518, "omega":-0.32269, "ax":-1.98771, "ay":2.90221, "alpha":-0.85428, "fx":[-26.32416,-21.50859,-27.82725,-32.53321], "fy":[40.24841,43.03246,39.25331,35.43612]},
-    {"t":10.3157, "x":2.97659, "y":4.12139, "heading":-0.06696, "vx":-0.82503, "vy":1.20465, "omega":-0.35492, "ax":-1.98752, "ay":2.90189, "alpha":-0.85395, "fx":[-26.4008,-21.48823,-27.76558,-32.52816], "fy":[40.19093,43.03685,39.2914,35.43387]},
-    {"t":10.35343, "x":2.94405, "y":4.16889, "heading":-0.08035, "vx":-0.9, "vy":1.31412, "omega":-0.38713, "ax":-1.9873, "ay":2.90153, "alpha":-0.85361, "fx":[-26.48502,-21.46671,-27.69754,-32.52161], "fy":[40.12726,43.04101,39.33311,35.4321]},
-    {"t":10.39115, "x":2.90869, "y":4.22052, "heading":-0.09496, "vx":-0.97497, "vy":1.42356, "omega":-0.41933, "ax":-1.98705, "ay":2.90112, "alpha":-0.85326, "fx":[-26.57672,-21.44424,-27.62305,-32.51322], "fy":[40.05718,43.04466,39.37828,35.43093]},
-    {"t":10.42887, "x":2.8705, "y":4.27629, "heading":-0.11077, "vx":-1.04992, "vy":1.533, "omega":-0.45151, "ax":-1.98676, "ay":2.90064, "alpha":-0.85291, "fx":[-26.67572,-21.42107,-27.542,-32.5026], "fy":[39.98045,43.04749,39.42675,35.43046]},
-    {"t":10.46659, "x":2.82948, "y":4.33618, "heading":-0.1278, "vx":-1.12486, "vy":1.64241, "omega":-0.48369, "ax":-1.98642, "ay":2.90009, "alpha":-0.85258, "fx":[-26.7818,-21.39741,-27.45423,-32.4893], "fy":[39.89677,43.04906,39.47828,35.43077]},
-    {"t":10.50431, "x":2.78564, "y":4.40019, "heading":-0.14605, "vx":-1.19979, "vy":1.75181, "omega":-0.51585, "ax":-1.98601, "ay":2.89943, "alpha":-0.85227, "fx":[-26.89464,-21.3735,-27.35958,-32.47279], "fy":[39.80577,43.04888,39.53256,35.43189]},
-    {"t":10.54203, "x":2.73897, "y":4.46834, "heading":-0.16551, "vx":-1.27471, "vy":1.86118, "omega":-0.54799, "ax":-1.98551, "ay":2.89864, "alpha":-0.85198, "fx":[-27.01381,-21.34952,-27.2578,-32.45244], "fy":[39.70694,43.04627,39.58918,35.43375]},
-    {"t":10.57975, "x":2.68947, "y":4.5406, "heading":-0.18618, "vx":-1.3496, "vy":1.97051, "omega":-0.58013, "ax":-1.9849, "ay":2.89768, "alpha":-0.85173, "fx":[-27.13868,-21.32561,-27.14857,-32.42745], "fy":[39.5996,43.04037,39.64754,35.43615]},
-    {"t":10.61747, "x":2.63715, "y":4.61699, "heading":-0.20806, "vx":-1.42447, "vy":2.07982, "omega":-0.61226, "ax":-1.98413, "ay":2.89647, "alpha":-0.85153, "fx":[-27.26836,-21.30178,-27.03138,-32.39679], "fy":[39.48276,43.02995,39.70677,35.43866]},
-    {"t":10.65519, "x":2.58201, "y":4.69751, "heading":-0.23116, "vx":-1.49932, "vy":2.18907, "omega":-0.64438, "ax":-1.98313, "ay":2.89493, "alpha":-0.85138, "fx":[-27.40149,-21.27782,-26.90547,-32.35901], "fy":[39.35488,43.01321,39.76555,35.44039]},
-    {"t":10.69291, "x":2.52404, "y":4.78214, "heading":-0.25546, "vx":-1.57412, "vy":2.29827, "omega":-0.67649, "ax":-1.98178, "ay":2.89287, "alpha":-0.8513, "fx":[-27.5359,-21.25302,-26.76957,-32.31197], "fy":[39.21341,42.98731,39.8217,35.43969]},
-    {"t":10.73063, "x":2.46326, "y":4.87089, "heading":-0.28098, "vx":-1.64888, "vy":2.4074, "omega":-0.70861, "ax":-1.97988, "ay":2.89, "alpha":-0.8513, "fx":[-27.66786,-21.22569,-26.62135,-32.2521], "fy":[39.05379,42.94737,39.87141,35.43328]},
-    {"t":10.76836, "x":2.39965, "y":4.96376, "heading":-0.30771, "vx":-1.72356, "vy":2.51641, "omega":-0.74072, "ax":-1.97701, "ay":2.88571, "alpha":-0.8514, "fx":[-27.79017,-21.1918,-26.45615,-32.17276], "fy":[38.86679,42.88395,39.90711,35.41423]},
-    {"t":10.80608, "x":2.33323, "y":5.06073, "heading":-0.33565, "vx":-1.79813, "vy":2.62526, "omega":-0.77283, "ax":-1.97221, "ay":2.87857, "alpha":-0.85171, "fx":[-27.88659,-21.14093,-26.26298,-32.05912], "fy":[38.63088,42.77561,39.91144,35.36581]},
-    {"t":10.8438, "x":2.264, "y":5.16181, "heading":-0.3648, "vx":-1.87253, "vy":2.73384, "omega":-0.80496, "ax":-1.9626, "ay":2.86436, "alpha":-0.85255, "fx":[-27.90958,-21.04,-26.00872,-31.86822], "fy":[38.28126,42.55929,39.83285,35.23698]},
-    {"t":10.88152, "x":2.19197, "y":5.26697, "heading":-0.39517, "vx":-1.94656, "vy":2.84189, "omega":-0.83712, "ax":-1.93395, "ay":2.82215, "alpha":-0.85596, "fx":[-27.62721,-20.71996,-25.52856,-31.39122], "fy":[37.49652,41.92633,39.41755,34.77248]},
-    {"t":10.91924, "x":2.11717, "y":5.37617, "heading":-0.42674, "vx":-2.01951, "vy":2.94834, "omega":-0.86941, "ax":-0.00042, "ay":-0.01101, "alpha":-0.31897, "fx":[0.43792,1.17801,-0.44951,-1.18931], "fy":[-1.33339,0.2938,1.03388,-0.59361]},
-    {"t":10.95696, "x":2.04099, "y":5.48738, "heading":-0.45954, "vx":-2.01953, "vy":2.94793, "omega":-0.88144, "ax":1.93378, "ay":-2.82317, "alpha":0.83891, "fx":[27.96018,20.88017,25.23542,31.18187], "fy":[-37.26208,-41.83335,-39.59994,-34.97304]},
-    {"t":10.99468, "x":1.96619, "y":5.59657, "heading":-0.49279, "vx":-1.94658, "vy":2.84144, "omega":-0.8498, "ax":1.96253, "ay":-2.86475, "alpha":0.8471, "fx":[28.64647,21.18008,25.39759,31.59839], "fy":[-37.74076,-42.47541,-40.22193,-35.4931]},
-    {"t":11.0324, "x":1.89416, "y":5.70171, "heading":-0.52484, "vx":-1.87255, "vy":2.73337, "omega":-0.81784, "ax":1.97218, "ay":-2.87869, "alpha":0.85145, "fx":[28.99969,21.3016,25.34931,31.69736], "fy":[-37.80916,-42.68147,-40.49508,-35.70441]},
-    {"t":11.07012, "x":1.82493, "y":5.80277, "heading":-0.55569, "vx":-1.79816, "vy":2.62479, "omega":-0.78572, "ax":1.97701, "ay":-2.88566, "alpha":0.85462, "fx":[29.26019,21.38338,25.2531,31.71392], "fy":[-37.77882,-42.77454,-40.67644,-35.83955]},
-    {"t":11.10784, "x":1.7585, "y":5.89973, "heading":-0.58533, "vx":-1.72359, "vy":2.51594, "omega":-0.75349, "ax":1.97989, "ay":-2.88983, "alpha":0.85718, "fx":[29.47706,21.45225,25.14177,31.69656], "fy":[-37.71299,-42.82066,-40.81803,-35.94471]},
-    {"t":11.14556, "x":1.6949, "y":5.99257, "heading":-0.61375, "vx":-1.6489, "vy":2.40693, "omega":-0.72115, "ax":1.9818, "ay":-2.8926, "alpha":0.85936, "fx":[29.66698,21.51648,25.02627,31.66203], "fy":[-37.63287,-42.8423,-40.93732,-36.03488]},
-    {"t":11.18329, "x":1.63411, "y":6.08131, "heading":-0.64096, "vx":-1.57415, "vy":2.29782, "omega":-0.68874, "ax":1.98316, "ay":-2.89458, "alpha":0.86124, "fx":[29.83731,21.57911,24.91127,31.61787], "fy":[-37.54753,-42.84936,-41.04189,-36.11621]},
-    {"t":11.22101, "x":1.57614, "y":6.16593, "heading":-0.66693, "vx":-1.49934, "vy":2.18863, "omega":-0.65625, "ax":1.98417, "ay":-2.89606, "alpha":0.86289, "fx":[29.99192,21.64129,24.79911,31.56809], "fy":[-37.46145,-42.84698,-41.13558,-36.19158]},
-    {"t":11.25873, "x":1.521, "y":6.24642, "heading":-0.69169, "vx":-1.4245, "vy":2.07939, "omega":-0.6237, "ax":1.98494, "ay":-2.89721, "alpha":0.86434, "fx":[30.13314,21.70335,24.69103,31.51512], "fy":[-37.37706,-42.83819,-41.22059,-36.26242]},
-    {"t":11.29645, "x":1.46868, "y":6.3228, "heading":-0.71522, "vx":-1.34962, "vy":1.97011, "omega":-0.5911, "ax":1.98556, "ay":-2.89813, "alpha":0.86562, "fx":[30.26249,21.76524,24.58777,31.46054], "fy":[-37.29575,-42.82491,-41.29829,-36.32947]},
-    {"t":11.33417, "x":1.41918, "y":6.39505, "heading":-0.73751, "vx":-1.27473, "vy":1.86079, "omega":-0.55845, "ax":1.98605, "ay":-2.89889, "alpha":0.86673, "fx":[30.38109,21.8267,24.48975,31.4055], "fy":[-37.21835,-42.80849,-41.36957,-36.39313]},
-    {"t":11.37189, "x":1.37251, "y":6.46318, "heading":-0.75858, "vx":-1.19981, "vy":1.75144, "omega":-0.52575, "ax":1.98646, "ay":-2.89952, "alpha":0.86771, "fx":[30.4898,21.88741,24.39722,31.35085], "fy":[-37.14533,-42.78992,-41.4351,-36.45357]},
-    {"t":11.40961, "x":1.32866, "y":6.52718, "heading":-0.77841, "vx":-1.12488, "vy":1.64207, "omega":-0.49302, "ax":1.9868, "ay":-2.90005, "alpha":0.86855, "fx":[30.58934,21.94698,24.31033,31.29724], "fy":[-37.07695,-42.76995,-41.49535,-36.51086]},
-    {"t":11.44733, "x":1.28765, "y":6.58706, "heading":-0.79701, "vx":-1.04993, "vy":1.53267, "omega":-0.46026, "ax":1.98709, "ay":-2.90052, "alpha":0.86928, "fx":[30.68029,22.00501,24.22916,31.24521], "fy":[-37.01335,-42.74919,-41.55069,-36.56501]},
-    {"t":11.48505, "x":1.24945, "y":6.64281, "heading":-0.81437, "vx":-0.97498, "vy":1.42326, "omega":-0.42747, "ax":1.98734, "ay":-2.90092, "alpha":0.8699, "fx":[30.76317,22.06112,24.15371,31.19521], "fy":[-36.95457,-42.72816,-41.60142,-36.61598]},
-    {"t":11.52277, "x":1.21409, "y":6.69443, "heading":-0.83049, "vx":-0.90001, "vy":1.31384, "omega":-0.39466, "ax":1.98756, "ay":-2.90127, "alpha":0.87043, "fx":[30.83844,22.11491,24.084,31.14762], "fy":[-36.9006,-42.70726,-41.6478,-36.66372]},
-    {"t":11.56049, "x":1.18156, "y":6.74193, "heading":-0.84538, "vx":-0.82504, "vy":1.2044, "omega":-0.36182, "ax":1.98775, "ay":-2.90159, "alpha":0.87087, "fx":[30.90652,22.16604,24.01998,31.10274], "fy":[-36.85138,-42.68688,-41.69002,-36.70817]},
-    {"t":11.59822, "x":1.15185, "y":6.78529, "heading":-0.85903, "vx":-0.75006, "vy":1.09495, "omega":-0.32897, "ax":1.98792, "ay":-2.90187, "alpha":0.87124, "fx":[30.96778,22.21416,23.96162,31.06085], "fy":[-36.80683,-42.66733,-41.72828,-36.74926]},
-    {"t":11.63594, "x":1.12497, "y":6.82453, "heading":-0.87144, "vx":-0.67508, "vy":0.98549, "omega":-0.29611, "ax":1.98807, "ay":-2.90212, "alpha":0.87155, "fx":[31.02255,22.25896,23.90887,31.0222], "fy":[-36.76686,-42.64889,-41.76273,-36.78693]},
-    {"t":11.67366, "x":1.10092, "y":6.85964, "heading":-0.88261, "vx":-0.60009, "vy":0.87602, "omega":-0.26323, "ax":1.9882, "ay":-2.90235, "alpha":0.87181, "fx":[31.07112,22.30016,23.86166,30.98698], "fy":[-36.73138,-42.6318,-41.7935,-36.82112]},
-    {"t":11.71138, "x":1.0797, "y":6.89062, "heading":-0.89254, "vx":-0.52509, "vy":0.76654, "omega":-0.23035, "ax":1.98832, "ay":-2.90255, "alpha":0.87201, "fx":[31.11377,22.33751,23.81994,30.95537], "fy":[-36.70027,-42.61628,-41.82071,-36.85176]},
-    {"t":11.7491, "x":1.06131, "y":6.91747, "heading":-0.90122, "vx":-0.45009, "vy":0.65705, "omega":-0.19745, "ax":1.98844, "ay":-2.90274, "alpha":0.87218, "fx":[31.15074,22.37077,23.78367,30.92753], "fy":[-36.67346,-42.60251,-41.84447,-36.87882]},
-    {"t":11.78682, "x":1.04574, "y":6.94019, "heading":-0.90867, "vx":-0.37508, "vy":0.54756, "omega":-0.16455, "ax":1.98854, "ay":-2.90291, "alpha":0.87231, "fx":[31.18223,22.39974,23.75277,30.90358], "fy":[-36.65084,-42.59066,-41.86487,-36.90225]},
-    {"t":11.82454, "x":1.03301, "y":6.95878, "heading":-0.91488, "vx":-0.30007, "vy":0.43806, "omega":-0.13165, "ax":1.98863, "ay":-2.90307, "alpha":0.87242, "fx":[31.20841,22.42427,23.72721,30.88363], "fy":[-36.63233,-42.58085,-41.88197,-36.92202]},
-    {"t":11.86226, "x":1.02311, "y":6.97324, "heading":-0.91985, "vx":-0.22506, "vy":0.32855, "omega":-0.09874, "ax":1.98872, "ay":-2.90321, "alpha":0.87251, "fx":[31.22945,22.44421,23.70695,30.86778], "fy":[-36.61788,-42.57322,-41.89586,-36.93809]},
-    {"t":11.89998, "x":1.01603, "y":6.98356, "heading":-0.92357, "vx":-0.15004, "vy":0.21904, "omega":-0.06583, "ax":1.98881, "ay":-2.90335, "alpha":0.87257, "fx":[31.24545,22.45946,23.69196,30.85609], "fy":[-36.60743,-42.56783,-41.90657,-36.95044]},
-    {"t":11.9377, "x":1.01179, "y":6.98976, "heading":-0.92605, "vx":-0.07502, "vy":0.10952, "omega":-0.03292, "ax":1.98889, "ay":-2.90347, "alpha":0.87262, "fx":[31.25652,22.46992,23.68221,30.84863], "fy":[-36.60094,-42.56479,-41.91415,-36.95906]},
-    {"t":11.97542, "x":1.01037, "y":6.99183, "heading":-0.9273, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.86512, "ay":-2.07538, "alpha":-0.10359, "fx":[38.56437,39.21104,39.41498,38.76117], "fy":[-28.8233,-27.93772,-27.64753,-28.55638]},
-    {"t":12.02039, "x":1.01327, "y":6.98973, "heading":-0.9273, "vx":0.12883, "vy":-0.09332, "omega":-0.00466, "ax":2.86499, "ay":-2.07528, "alpha":-0.10359, "fx":[38.56265,39.20928,39.41316,38.75939], "fy":[-28.82198,-27.93648,-27.64628,-28.55504]},
-    {"t":12.06535, "x":1.02196, "y":6.98343, "heading":-0.9275, "vx":0.25765, "vy":-0.18663, "omega":-0.00932, "ax":2.86485, "ay":-2.07518, "alpha":-0.10358, "fx":[38.56069,39.20724,39.41119,38.75752], "fy":[-28.82058,-27.93523,-27.64484,-28.55345]},
-    {"t":12.11032, "x":1.03644, "y":6.97294, "heading":-0.92792, "vx":0.38647, "vy":-0.27994, "omega":-0.01397, "ax":2.86469, "ay":-2.07506, "alpha":-0.10358, "fx":[38.55848,39.2049,39.40904,38.75551], "fy":[-28.81909,-27.93395,-27.64318,-28.55157]},
-    {"t":12.15528, "x":1.05671, "y":6.95826, "heading":-0.92855, "vx":0.51528, "vy":-0.37325, "omega":-0.01863, "ax":2.86451, "ay":-2.07493, "alpha":-0.10357, "fx":[38.55596,39.20222,39.40667,38.75334], "fy":[-28.81747,-27.93262,-27.64128,-28.54937]},
-    {"t":12.20025, "x":1.08278, "y":6.93938, "heading":-0.92939, "vx":0.64408, "vy":-0.46655, "omega":-0.02329, "ax":2.86431, "ay":-2.07479, "alpha":-0.10357, "fx":[38.5531,39.19914,39.40404,38.75095], "fy":[-28.81569,-27.93119,-27.6391,-28.54682]},
-    {"t":12.24521, "x":1.11463, "y":6.9163, "heading":-0.93044, "vx":0.77287, "vy":-0.55984, "omega":-0.02794, "ax":2.86408, "ay":-2.07462, "alpha":-0.10356, "fx":[38.54984,39.19562,39.40108,38.74829], "fy":[-28.81371,-27.92963,-27.6366,-28.54388]},
-    {"t":12.29018, "x":1.15228, "y":6.88903, "heading":-0.93169, "vx":0.90166, "vy":-0.65312, "omega":-0.0326, "ax":2.86382, "ay":-2.07443, "alpha":-0.10355, "fx":[38.54609,39.19157,39.3977,38.74527], "fy":[-28.81146,-27.92788,-27.63373,-28.54048]},
-    {"t":12.33514, "x":1.19572, "y":6.85757, "heading":-0.93316, "vx":1.03043, "vy":-0.7464, "omega":-0.03726, "ax":2.86352, "ay":-2.07421, "alpha":-0.10355, "fx":[38.54177,39.18689,39.39382,38.7418], "fy":[-28.80886,-27.92588,-27.63041,-28.53654]},
-    {"t":12.38011, "x":1.24495, "y":6.82191, "heading":-0.93483, "vx":1.15918, "vy":-0.83967, "omega":-0.04191, "ax":2.86317, "ay":-2.07396, "alpha":-0.10354, "fx":[38.53674,39.18144,39.38927,38.73773], "fy":[-28.80583,-27.92351,-27.62654,-28.53197]},
-    {"t":12.42507, "x":1.29996, "y":6.78206, "heading":-0.93672, "vx":1.28793, "vy":-0.93292, "omega":-0.04657, "ax":2.86275, "ay":-2.07366, "alpha":-0.10353, "fx":[38.53081,39.17504,39.38388,38.73288], "fy":[-28.80221,-27.92067,-27.62201,-28.52663]},
-    {"t":12.47004, "x":1.36077, "y":6.73801, "heading":-0.93881, "vx":1.41665, "vy":-1.02616, "omega":-0.05122, "ax":2.86225, "ay":-2.0733, "alpha":-0.10351, "fx":[38.52374,39.16744,39.37738,38.72698], "fy":[-28.79782,-27.91715,-27.61661,-28.52032]},
-    {"t":12.515, "x":1.42736, "y":6.68978, "heading":-0.94112, "vx":1.54535, "vy":-1.11939, "omega":-0.05588, "ax":2.86165, "ay":-2.07286, "alpha":-0.1035, "fx":[38.51515,39.15826,39.36938,38.71967], "fy":[-28.79237,-27.9127,-27.6101,-28.51276]},
-    {"t":12.55997, "x":1.49974, "y":6.63735, "heading":-0.94363, "vx":1.67402, "vy":-1.21259, "omega":-0.06053, "ax":2.86089, "ay":-2.07231, "alpha":-0.10348, "fx":[38.5045,39.14693,39.35931,38.71036], "fy":[-28.78546,-27.90693,-27.60207,-28.50353]},
-    {"t":12.60493, "x":1.5779, "y":6.58073, "heading":-0.94635, "vx":1.80266, "vy":-1.30577, "omega":-0.06519, "ax":2.85991, "ay":-2.0716, "alpha":-0.10346, "fx":[38.49092,39.1326,39.34626,38.69818], "fy":[-28.77642,-27.8992,-27.59189,-28.49198]},
-    {"t":12.6499, "x":1.66185, "y":6.51992, "heading":-0.94928, "vx":1.93126, "vy":-1.39892, "omega":-0.06984, "ax":2.85861, "ay":-2.07066, "alpha":-0.10344, "fx":[38.47297,39.11378,39.32873,38.68163], "fy":[-28.76418,-27.88848,-27.57851,-28.477]},
-    {"t":12.69486, "x":1.75158, "y":6.45493, "heading":-0.95242, "vx":2.05979, "vy":-1.49203, "omega":-0.07449, "ax":2.85679, "ay":-2.06934, "alpha":-0.10341, "fx":[38.44805,39.08787,39.30402,38.65803], "fy":[-28.74677,-27.8729,-27.56006,-28.45662]},
-    {"t":12.73983, "x":1.84709, "y":6.38574, "heading":-0.95577, "vx":2.18825, "vy":-1.58508, "omega":-0.07914, "ax":2.85406, "ay":-2.06736, "alpha":-0.10338, "fx":[38.41098,39.0496,39.26673,38.62206], "fy":[-28.7203,-27.84874,-27.53277,-28.42691]},
-    {"t":12.78479, "x":1.94836, "y":6.31238, "heading":-0.95933, "vx":2.31658, "vy":-1.67804, "omega":-0.08379, "ax":2.84951, "ay":-2.06407, "alpha":-0.10334, "fx":[38.34966,38.98679,39.20429,38.56125], "fy":[-28.67571,-27.80727,-27.48784,-28.37871]},
-    {"t":12.82975, "x":2.05541, "y":6.23484, "heading":-0.9631, "vx":2.44471, "vy":-1.77085, "omega":-0.08843, "ax":2.84044, "ay":-2.0575, "alpha":-0.10328, "fx":[38.22792,38.86291,39.07916,38.43839], "fy":[-28.58591,-27.72248,-27.39899,-28.2846]},
-    {"t":12.87472, "x":2.16821, "y":6.15314, "heading":-0.96707, "vx":2.57243, "vy":-1.86336, "omega":-0.09308, "ax":2.81341, "ay":-2.03792, "alpha":-0.10319, "fx":[37.86622,38.49686,38.70509,38.06881], "fy":[-28.31659,-27.46514,-27.13571,-28.00872]},
-    {"t":12.91968, "x":2.28672, "y":6.06729, "heading":-0.97126, "vx":2.69893, "vy":-1.95499, "omega":-0.09772, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[0.00006,-0.00033,-0.00006,0.00033], "fy":[0.00033,0.00006,-0.00033,-0.00007]},
-    {"t":12.96465, "x":2.40808, "y":5.97939, "heading":-0.97565, "vx":2.69893, "vy":-1.95499, "omega":-0.09771, "ax":-2.81341, "ay":2.03792, "alpha":0.1032, "fx":[-37.86431,-38.49334,-38.70685,-38.07249], "fy":[28.31895,27.47017,27.13342,28.00362]},
-    {"t":13.00961, "x":2.52659, "y":5.89354, "heading":-0.98005, "vx":2.57243, "vy":-1.86336, "omega":-0.09307, "ax":-2.84044, "ay":2.0575, "alpha":0.10328, "fx":[-38.22422,-38.85583,-39.08255,-38.4458], "fy":[28.59066,27.7325,27.39438,28.27444]},
-    {"t":13.05458, "x":2.63939, "y":5.81183, "heading":-0.98423, "vx":2.44471, "vy":-1.77085, "omega":-0.08843, "ax":-2.84951, "ay":2.06407, "alpha":0.10334, "fx":[-38.34426,-38.97631,-39.20923,-38.57219], "fy":[28.68274,27.82204,27.48101,28.36373]},
-    {"t":13.09954, "x":2.74643, "y":5.7343, "heading":-0.98821, "vx":2.31658, "vy":-1.67804, "omega":-0.08378, "ax":-2.85406, "ay":2.06736, "alpha":0.10338, "fx":[-38.40396,-39.03592,-39.27313,-38.63636], "fy":[28.72949,27.86801,27.52384,28.40738]},
-    {"t":13.14451, "x":2.84771, "y":5.66093, "heading":-0.99197, "vx":2.18825, "vy":-1.58508, "omega":-0.07913, "ax":-2.85679, "ay":2.06934, "alpha":0.10341, "fx":[-38.43951,-39.07114,-39.31181,-38.67551], "fy":[28.75799,27.89643,27.54915,28.43278]},
-    {"t":13.18947, "x":2.94322, "y":5.59175, "heading":-0.99553, "vx":2.05979, "vy":-1.49203, "omega":-0.07448, "ax":-2.85861, "ay":2.07066, "alpha":0.10344, "fx":[-38.463,-39.0942,-39.33782,-38.70209], "fy":[28.77732,27.91601,27.56574,28.44909]},
-    {"t":13.23444, "x":3.03294, "y":5.52676, "heading":-0.99888, "vx":1.93126, "vy":-1.39892, "omega":-0.06983, "ax":-2.85991, "ay":2.0716, "alpha":0.10346, "fx":[-38.47961,-39.11033,-39.35657,-38.72144], "fy":[28.79137,27.93049,27.57737,28.46027]},
-    {"t":13.2794, "x":3.11689, "y":5.46595, "heading":-1.00202, "vx":1.80266, "vy":-1.30577, "omega":-0.06518, "ax":-2.86089, "ay":2.07231, "alpha":0.10348, "fx":[-38.49194,-39.12216,-39.37076,-38.73625], "fy":[28.80208,27.94172,27.58592,28.46826]},
-    {"t":13.32437, "x":3.19506, "y":5.40933, "heading":-1.00495, "vx":1.67402, "vy":-1.21259, "omega":-0.06053, "ax":-2.86165, "ay":2.07286, "alpha":0.1035, "fx":[-38.50143,-39.13116,-39.38189,-38.74798], "fy":[28.81055,27.95076,27.59244,28.47419]},
-    {"t":13.36933, "x":3.26743, "y":5.3569, "heading":-1.00768, "vx":1.54535, "vy":-1.11939, "omega":-0.05588, "ax":-2.86225, "ay":2.0733, "alpha":0.10351, "fx":[-38.50895,-39.13819,-39.39086,-38.75754], "fy":[28.81743,27.95822,27.59756,28.47869]},
-    {"t":13.4143, "x":3.33403, "y":5.30867, "heading":-1.01019, "vx":1.41665, "vy":-1.02616, "omega":-0.05122, "ax":-2.86275, "ay":2.07366, "alpha":0.10352, "fx":[-38.51504,-39.14382,-39.39825,-38.7655], "fy":[28.82314,27.96449,27.60168,28.48221]},
-    {"t":13.45926, "x":3.39483, "y":5.26462, "heading":-1.01249, "vx":1.28793, "vy":-0.93292, "omega":-0.04657, "ax":-2.86317, "ay":2.07396, "alpha":0.10353, "fx":[-38.52008,-39.14843,-39.40445,-38.77222], "fy":[28.82795,27.96985,27.60506,28.485]},
-    {"t":13.50423, "x":3.44985, "y":5.22477, "heading":-1.01458, "vx":1.15918, "vy":-0.83967, "omega":-0.04191, "ax":-2.86352, "ay":2.07421, "alpha":0.10354, "fx":[-38.52432,-39.15226,-39.40972,-38.77798], "fy":[28.83206,27.97447,27.60788,28.48728]},
-    {"t":13.54919, "x":3.49908, "y":5.18911, "heading":-1.01647, "vx":1.03043, "vy":-0.7464, "omega":-0.03726, "ax":-2.86382, "ay":2.07443, "alpha":0.10355, "fx":[-38.52793,-39.1555,-39.41425,-38.78295], "fy":[28.8356,27.97849,27.61028,28.48918]},
-    {"t":13.59416, "x":3.54251, "y":5.15765, "heading":-1.01814, "vx":0.90166, "vy":-0.65312, "omega":-0.0326, "ax":-2.86408, "ay":2.07462, "alpha":0.10355, "fx":[-38.53106,-39.1583,-39.41818,-38.78729], "fy":[28.83868,27.982,27.61235,28.49079]},
-    {"t":13.63912, "x":3.58016, "y":5.13038, "heading":-1.01961, "vx":0.77287, "vy":-0.55984, "omega":-0.02794, "ax":-2.86431, "ay":2.07479, "alpha":0.10356, "fx":[-38.53379,-39.16074,-39.42163,-38.79108], "fy":[28.84138,27.98507,27.61416,28.49221]},
-    {"t":13.68408, "x":3.61202, "y":5.1073, "heading":-1.02087, "vx":0.64408, "vy":-0.46655, "omega":-0.02329, "ax":-2.86451, "ay":2.07493, "alpha":0.10357, "fx":[-38.53621,-39.16291,-39.42466,-38.79441], "fy":[28.84375,27.98776,27.61576,28.49348]},
-    {"t":13.72905, "x":3.63808, "y":5.08842, "heading":-1.02191, "vx":0.51528, "vy":-0.37325, "omega":-0.01863, "ax":-2.86469, "ay":2.07506, "alpha":0.10357, "fx":[-38.53838,-39.16487,-39.42734,-38.79733], "fy":[28.84584,27.9901,27.61721,28.49465]},
-    {"t":13.77401, "x":3.65836, "y":5.07373, "heading":-1.02275, "vx":0.38647, "vy":-0.27994, "omega":-0.01397, "ax":-2.86485, "ay":2.07518, "alpha":0.10358, "fx":[-38.54034,-39.16667,-39.42972,-38.79991], "fy":[28.84768,27.99214,27.61853,28.49576]},
-    {"t":13.81898, "x":3.67284, "y":5.06324, "heading":-1.02338, "vx":0.25765, "vy":-0.18663, "omega":-0.00932, "ax":-2.86499, "ay":2.07528, "alpha":0.10358, "fx":[-38.54212,-39.16834,-39.43184,-38.80217], "fy":[28.84931,27.9939,27.61974,28.49684]},
-    {"t":13.86394, "x":3.68153, "y":5.05695, "heading":-1.0238, "vx":0.12883, "vy":-0.09332, "omega":-0.00466, "ax":-2.86512, "ay":2.07538, "alpha":0.10358, "fx":[-38.54377,-39.16992,-39.43374,-38.80415], "fy":[28.85074,27.9954,27.62088,28.4979]},
-    {"t":13.90891, "x":3.68442, "y":5.05485, "heading":-1.02401, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
-  "splits":[0]
- },
- "events":[]
-}
diff --git a/src/main/deploy/pathplanner/autos/AutoLeft.auto b/src/main/deploy/pathplanner/autos/AutoLeft.auto
new file mode 100644
index 0000000..8524c50
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/AutoLeft.auto
@@ -0,0 +1,55 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "PreloadLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stopShoot"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 3.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HangLeft1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HangLeft2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "hang"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": false,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/DepotCollect.auto b/src/main/deploy/pathplanner/autos/DepotCollect.auto
new file mode 100644
index 0000000..90723c6
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/DepotCollect.auto
@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DepotCenter"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DepotShootCenter"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/LeftCollect.auto b/src/main/deploy/pathplanner/autos/LeftCollect.auto
new file mode 100644
index 0000000..dee60e2
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/LeftCollect.auto
@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TrenchLeft"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SwipeLeft"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "ReturnTrenchLeft"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/RightCollect.auto b/src/main/deploy/pathplanner/autos/RightCollect.auto
new file mode 100644
index 0000000..82e7fe3
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/RightCollect.auto
@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "TrenchRight"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SwipeRight"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "ReturnTrenchRight"
+          }
+        }
+      ]
+    }
+  },
+  "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 7a1e1ce..ac5f521 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/DepotCenter.path b/src/main/deploy/pathplanner/paths/DepotCenter.path
new file mode 100644
index 0000000..399259c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/DepotCenter.path
@@ -0,0 +1,68 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.45681883024251,
+        "y": 4.403751783166904
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.9263338088445074,
+        "y": 4.597831669044223
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7914550641940081,
+        "y": 6.072838801711842
+      },
+      "prevControl": {
+        "x": 2.564051355206847,
+        "y": 6.085777460770328
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DepotCenterEnd"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.8669817690749491,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 5.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Alliance Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -36.02737338510356
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/DepotShootCenter.path b/src/main/deploy/pathplanner/paths/DepotShootCenter.path
new file mode 100644
index 0000000..cab67a4
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/DepotShootCenter.path
@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7914550641940081,
+        "y": 6.072838801711842
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7914550641940081,
+        "y": 6.072838801711842
+      },
+      "isLocked": false,
+      "linkedName": "DepotCenterEnd"
+    },
+    {
+      "anchor": {
+        "x": 2.8487018544935796,
+        "y": 4.015592011412268
+      },
+      "prevControl": {
+        "x": 2.7840085592011405,
+        "y": 5.1800713266761775
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DepotShootCenterEnd"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.623,
+        "y": 4.05
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.25,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Alliance Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/HangLeft.path b/src/main/deploy/pathplanner/paths/HangLeft.path
new file mode 100644
index 0000000..b826858
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/HangLeft.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.154166666666667,
+        "y": 5.067911111111112
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.325888888888889,
+        "y": 5.564877777777777
+      },
+      "isLocked": false,
+      "linkedName": "ReturnTrenchLeftEnd"
+    },
+    {
+      "anchor": {
+        "x": 1.6632666666666664,
+        "y": 4.678133333333332
+      },
+      "prevControl": {
+        "x": 2.4428222222222193,
+        "y": 4.668388888888886
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -88.51854282911293
+  },
+  "reversed": false,
+  "folder": "Neutral Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -27.28121112254839
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/HangLeft1.path b/src/main/deploy/pathplanner/paths/HangLeft1.path
new file mode 100644
index 0000000..b93dc3a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/HangLeft1.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.29,
+        "y": 2.017
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.924538197409069,
+        "y": 2.3929658255457746
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.06,
+        "y": 2.017
+      },
+      "prevControl": {
+        "x": 1.0830664619292123,
+        "y": 1.7680663977401438
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Hang Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 45.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/HangLeft2.path b/src/main/deploy/pathplanner/paths/HangLeft2.path
new file mode 100644
index 0000000..82d224e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/HangLeft2.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.06,
+        "y": 2.017
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.0662045517242758,
+        "y": 2.772060322299513
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.06,
+        "y": 2.95
+      },
+      "prevControl": {
+        "x": 1.0482405717754884,
+        "y": 2.700276721453861
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Hang Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/NeutralReturnTrenchLeft.path b/src/main/deploy/pathplanner/paths/NeutralReturnTrenchLeft.path
new file mode 100644
index 0000000..4d1df3f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/NeutralReturnTrenchLeft.path
@@ -0,0 +1,102 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.1775421472937,
+        "y": 7.416322222222221
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.1796913576946055,
+        "y": 7.314602720089652
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3393111111111113,
+        "y": 7.416322222222221
+      },
+      "prevControl": {
+        "x": 3.55850891816058,
+        "y": 7.536540030323617
+      },
+      "nextControl": {
+        "x": 3.120113304061643,
+        "y": 7.296104414120825
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.382910128388017,
+        "y": 6.512753209700428
+      },
+      "prevControl": {
+        "x": 2.220629382465464,
+        "y": 7.249190195604299
+      },
+      "nextControl": {
+        "x": 2.4557888794833342,
+        "y": 6.182026311152107
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.154166666666667,
+        "y": 5.067911111111112
+      },
+      "prevControl": {
+        "x": 2.9444535373164356,
+        "y": 5.212507580753187
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "ReturnTrenchLeftEnd"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.1816976127320966,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.62,
+        "y": 4.039
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 2.2,
+      "maxWaypointRelativePos": 3.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -27.28121112254839
+  },
+  "reversed": false,
+  "folder": "Neutral Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/NeutralReturnTrenchRight.path b/src/main/deploy/pathplanner/paths/NeutralReturnTrenchRight.path
new file mode 100644
index 0000000..c3856be
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/NeutralReturnTrenchRight.path
@@ -0,0 +1,106 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.931333333333333,
+        "y": 0.5269999999999992
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.382924599438744,
+        "y": 0.5269999999999994
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.379186875891583,
+        "y": 0.5269999999999992
+      },
+      "prevControl": {
+        "x": 4.2590156918687585,
+        "y": 0.5221540656205423
+      },
+      "nextControl": {
+        "x": 3.129190667808615,
+        "y": 0.5283769328731714
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.9522111269614832,
+        "y": 1.013823109843081
+      },
+      "prevControl": {
+        "x": 2.9723228063699474,
+        "y": 0.6953053819320217
+      },
+      "nextControl": {
+        "x": 2.9026908586211704,
+        "y": 1.7980979104181296
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1931444444444446,
+        "y": 2.9825999999999997
+      },
+      "prevControl": {
+        "x": 2.971754978667799,
+        "y": 0.7916137458990562
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.0,
+      "rotationDegrees": 177.87659887200687
+    },
+    {
+      "waypointRelativePos": 2.0021321961620533,
+      "rotationDegrees": 179.39144110888557
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.63,
+        "y": 4.03
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 2.41,
+      "maxWaypointRelativePos": 3.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 34.835830364154894
+  },
+  "reversed": false,
+  "folder": "Neutral Right",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/PreloadLeft.path b/src/main/deploy/pathplanner/paths/PreloadLeft.path
new file mode 100644
index 0000000..971e07d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/PreloadLeft.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.7217407078598486,
+        "y": 0.36528959517045334
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.491448554596527,
+        "y": 0.055810175445162036
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.29,
+        "y": 2.017
+      },
+      "prevControl": {
+        "x": 2.1843002308162807,
+        "y": 1.76913764224765
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 45.0
+  },
+  "reversed": false,
+  "folder": "Hang Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path b/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path
new file mode 100644
index 0000000..20d568d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path
@@ -0,0 +1,122 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.665844444444444,
+        "y": 4.765833333333332
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.207357766605293,
+        "y": 5.841058521048764
+      },
+      "isLocked": false,
+      "linkedName": "SwipeLeftEnd"
+    },
+    {
+      "anchor": {
+        "x": 5.931333333333333,
+        "y": 7.416322222222221
+      },
+      "prevControl": {
+        "x": 6.7162227377693515,
+        "y": 7.202279252827075
+      },
+      "nextControl": {
+        "x": 5.615054102478352,
+        "y": 7.502573030740725
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3393111111111113,
+        "y": 7.416322222222221
+      },
+      "prevControl": {
+        "x": 3.687662243109644,
+        "y": 7.421700229689098
+      },
+      "nextControl": {
+        "x": 2.7760581588108626,
+        "y": 7.407626460087526
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.382910128388017,
+        "y": 6.512753209700428
+      },
+      "prevControl": {
+        "x": 2.402438844725988,
+        "y": 6.872100722346741
+      },
+      "nextControl": {
+        "x": 2.3580322232939324,
+        "y": 6.0549753834114455
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.154166666666667,
+        "y": 5.067911111111112
+      },
+      "prevControl": {
+        "x": 2.9486968238238505,
+        "y": 5.376805917860191
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "ReturnTrenchLeftEnd"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 3.0,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.62,
+        "y": 4.039
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 3.2,
+      "maxWaypointRelativePos": 4.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -27.28121112254839
+  },
+  "reversed": false,
+  "folder": "Neutral Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 122.27564431457749
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path
new file mode 100644
index 0000000..1eb63f1
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path
@@ -0,0 +1,122 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.700699001426534,
+        "y": 3.3723777777777775
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.033137820589351,
+        "y": 1.9842213303399618
+      },
+      "isLocked": false,
+      "linkedName": "SwipeRightEnd"
+    },
+    {
+      "anchor": {
+        "x": 5.931333333333333,
+        "y": 0.5269999999999992
+      },
+      "prevControl": {
+        "x": 6.989072753209699,
+        "y": 0.4962767475035662
+      },
+      "nextControl": {
+        "x": 5.383155793497732,
+        "y": 0.5429224442738556
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.379186875891583,
+        "y": 0.5269999999999992
+      },
+      "prevControl": {
+        "x": 4.2590156918687585,
+        "y": 0.5221540656205423
+      },
+      "nextControl": {
+        "x": 3.129190667808615,
+        "y": 0.5283769328731714
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.9522111269614832,
+        "y": 1.013823109843081
+      },
+      "prevControl": {
+        "x": 2.9723228063699474,
+        "y": 0.6953053819320217
+      },
+      "nextControl": {
+        "x": 2.9026908586211704,
+        "y": 1.7980979104181296
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1931444444444446,
+        "y": 2.9825999999999997
+      },
+      "prevControl": {
+        "x": 2.971754978667799,
+        "y": 0.7916137458990562
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "ReturnTrenchRightEnd"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.9792531120331951,
+      "rotationDegrees": 177.87659887200687
+    },
+    {
+      "waypointRelativePos": 3.0021321961620533,
+      "rotationDegrees": 179.39144110888557
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.63,
+        "y": 4.03
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 3.41,
+      "maxWaypointRelativePos": 4.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 34.835830364154894
+  },
+  "reversed": false,
+  "folder": "Neutral Right",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -115.11483488614444
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/ShootLeftToTrenchLeft.path b/src/main/deploy/pathplanner/paths/ShootLeftToTrenchLeft.path
new file mode 100644
index 0000000..2a5e8b8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/ShootLeftToTrenchLeft.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.154166666666667,
+        "y": 5.067911111111112
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.5732665782842257,
+        "y": 5.966236275955364
+      },
+      "isLocked": false,
+      "linkedName": "ReturnTrenchLeftEnd"
+    },
+    {
+      "anchor": {
+        "x": 3.622,
+        "y": 7.562488888888888
+      },
+      "prevControl": {
+        "x": 3.4931492650157048,
+        "y": 7.303448730289524
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TrenchLeftStart"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 23.025492008528023
+  },
+  "reversed": false,
+  "folder": "Neutral Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -27.28121112254839
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/ShootLeftToTrenchRight.path b/src/main/deploy/pathplanner/paths/ShootLeftToTrenchRight.path
new file mode 100644
index 0000000..8c452fd
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/ShootLeftToTrenchRight.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.154166666666667,
+        "y": 5.067911111111112
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0790949423247556,
+        "y": 4.505803016858917
+      },
+      "isLocked": false,
+      "linkedName": "ReturnTrenchLeftEnd"
+    },
+    {
+      "anchor": {
+        "x": 3.7026533523537797,
+        "y": 0.6256633380884444
+      },
+      "prevControl": {
+        "x": 2.7026533523537797,
+        "y": 0.6256633380884447
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TrenchRightStart"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -32.90524292298786
+  },
+  "reversed": false,
+  "folder": "Neutral Right",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -27.28121112254839
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/ShootRightToTrenchLeft.path b/src/main/deploy/pathplanner/paths/ShootRightToTrenchLeft.path
new file mode 100644
index 0000000..90f284c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/ShootRightToTrenchLeft.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.1931444444444446,
+        "y": 2.9825999999999997
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8215616681455185,
+        "y": 3.4837178349600704
+      },
+      "isLocked": false,
+      "linkedName": "ReturnTrenchRightEnd"
+    },
+    {
+      "anchor": {
+        "x": 3.622,
+        "y": 7.562488888888888
+      },
+      "prevControl": {
+        "x": 3.24810115350488,
+        "y": 7.346716947648624
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TrenchLeftStart"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 23.025492008528023
+  },
+  "reversed": false,
+  "folder": "Neutral Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 34.835830364154894
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/ShootRightToTrenchRight.path b/src/main/deploy/pathplanner/paths/ShootRightToTrenchRight.path
new file mode 100644
index 0000000..35a4f77
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/ShootRightToTrenchRight.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.1931444444444446,
+        "y": 2.9825999999999997
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2320053238686772,
+        "y": 1.8821827861579403
+      },
+      "isLocked": false,
+      "linkedName": "ReturnTrenchRightEnd"
+    },
+    {
+      "anchor": {
+        "x": 3.7026533523537797,
+        "y": 0.6256633380884444
+      },
+      "prevControl": {
+        "x": 3.030807453416149,
+        "y": 0.6106122448979577
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TrenchRightStart"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -32.90524292298786
+  },
+  "reversed": false,
+  "folder": "Neutral Right",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 34.835830364154894
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/StationCenter.path b/src/main/deploy/pathplanner/paths/StationCenter.path
new file mode 100644
index 0000000..5d62864
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/StationCenter.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.8487018544935796,
+        "y": 4.015592011412268
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2016666534554235,
+        "y": 3.3233789905366047
+      },
+      "isLocked": false,
+      "linkedName": "DepotShootCenterEnd"
+    },
+    {
+      "anchor": {
+        "x": 0.5973751783166898,
+        "y": 0.8973751783166914
+      },
+      "prevControl": {
+        "x": 1.0680613214397714,
+        "y": 1.5780987777774225
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "StationCenterEnd"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Alliance Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/StationShootCenter.path b/src/main/deploy/pathplanner/paths/StationShootCenter.path
new file mode 100644
index 0000000..655635a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/StationShootCenter.path
@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.5973751783166898,
+        "y": 0.8973751783166914
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.4786361957748682,
+        "y": 1.4318016192691616
+      },
+      "isLocked": false,
+      "linkedName": "StationCenterEnd"
+    },
+    {
+      "anchor": {
+        "x": 3.2368616262482157,
+        "y": 4.028530670470756
+      },
+      "prevControl": {
+        "x": 2.242255154397988,
+        "y": 4.030653058607221
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.623,
+        "y": 4.05
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.49155975692099935,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Alliance Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/SwipeLeft.path b/src/main/deploy/pathplanner/paths/SwipeLeft.path
new file mode 100644
index 0000000..0e85076
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/SwipeLeft.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.617122222222221,
+        "y": 6.714722222222222
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.617122222222221,
+        "y": 5.714722222222221
+      },
+      "isLocked": false,
+      "linkedName": "TrenchLeftEnd"
+    },
+    {
+      "anchor": {
+        "x": 7.665844444444444,
+        "y": 4.765833333333332
+      },
+      "prevControl": {
+        "x": 7.665844444444444,
+        "y": 5.765833333333332
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SwipeLeftEnd"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 122.27564431457749
+  },
+  "reversed": false,
+  "folder": "Neutral Left",
+  "idealStartingState": {
+    "velocity": 2.0,
+    "rotation": 116.56505117707796
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/SwipeRight.path b/src/main/deploy/pathplanner/paths/SwipeRight.path
new file mode 100644
index 0000000..9bdbfc7
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/SwipeRight.path
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.700699001426534,
+        "y": 1.1302710413694728
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.700699001426534,
+        "y": 2.1302710413694728
+      },
+      "isLocked": false,
+      "linkedName": "TrenchRightEnd"
+    },
+    {
+      "anchor": {
+        "x": 7.700699001426534,
+        "y": 3.3723777777777775
+      },
+      "prevControl": {
+        "x": 7.700699001426534,
+        "y": 2.3723777777777775
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SwipeRightEnd"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -115.11483488614444
+  },
+  "reversed": false,
+  "folder": "Neutral Right",
+  "idealStartingState": {
+    "velocity": 2.0,
+    "rotation": -104.03624346792651
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TrenchLeft.path b/src/main/deploy/pathplanner/paths/TrenchLeft.path
new file mode 100644
index 0000000..69f13a3
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TrenchLeft.path
@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.622,
+        "y": 7.562488888888888
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.791481184057424,
+        "y": 7.511295674470193
+      },
+      "isLocked": false,
+      "linkedName": "TrenchLeftStart"
+    },
+    {
+      "anchor": {
+        "x": 6.2236666666666665,
+        "y": 7.562488888888888
+      },
+      "prevControl": {
+        "x": 5.754164134386084,
+        "y": 7.5540188679051194
+      },
+      "nextControl": {
+        "x": 6.5900361678797,
+        "y": 7.569098347091411
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.617122222222221,
+        "y": 6.714722222222222
+      },
+      "prevControl": {
+        "x": 7.617122222222221,
+        "y": 7.377416090897831
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TrenchLeftEnd"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.0,
+    "rotation": 116.56505117707796
+  },
+  "reversed": false,
+  "folder": "Neutral Left",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 23.025492008528023
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/TrenchRight.path b/src/main/deploy/pathplanner/paths/TrenchRight.path
new file mode 100644
index 0000000..8359288
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/TrenchRight.path
@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.7026533523537797,
+        "y": 0.6256633380884444
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.186875891583452,
+        "y": 0.612724679029957
+      },
+      "isLocked": false,
+      "linkedName": "TrenchRightStart"
+    },
+    {
+      "anchor": {
+        "x": 6.290385164051354,
+        "y": 0.5221540656205423
+      },
+      "prevControl": {
+        "x": 5.977957318609738,
+        "y": 0.5221540656205423
+      },
+      "nextControl": {
+        "x": 6.602813009492971,
+        "y": 0.5221540656205423
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.700699001426534,
+        "y": 1.1302710413694728
+      },
+      "prevControl": {
+        "x": 7.7265763195435095,
+        "y": 0.40570613409415135
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "TrenchRightEnd"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.06396588486140724,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 540.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.0,
+    "rotation": -104.03624346792651
+  },
+  "reversed": false,
+  "folder": "Neutral Right",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -32.90524292298786
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/testPath.path b/src/main/deploy/pathplanner/paths/testPath.path
deleted file mode 100644
index 7ecb284..0000000
--- a/src/main/deploy/pathplanner/paths/testPath.path
+++ /dev/null
@@ -1,91 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 0.0,
-        "y": 0.0
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 1.6543032786885246,
-        "y": 0.15896516393442653
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.870081967215316,
-        "y": 0.7583504098420167
-      },
-      "prevControl": {
-        "x": 2.025922131149742,
-        "y": 0.386731557383
-      },
-      "nextControl": {
-        "x": 1.5354564647707203,
-        "y": 1.5563035310560531
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.4265368852459013,
-        "y": 1.6094774590163934
-      },
-      "prevControl": {
-        "x": 1.798155737707119,
-        "y": 1.6574282786944752
-      },
-      "nextControl": {
-        "x": 0.3915104179993174,
-        "y": 1.4759256567752452
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 0.0,
-        "y": 0.0
-      },
-      "prevControl": {
-        "x": 0.45553278688744697,
-        "y": 0.9501536885305403
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 1.57,
-      "rotationDegrees": 180.0
-    }
-  ],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/testPath2.path b/src/main/deploy/pathplanner/paths/testPath2.path
deleted file mode 100644
index 3f70b5f..0000000
--- a/src/main/deploy/pathplanner/paths/testPath2.path
+++ /dev/null
@@ -1,203 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 0.0,
-        "y": 0.0
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 1.6543032786885246,
-        "y": 0.15896516393442653
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.485761730491173,
-        "y": 4.037301172765461
-      },
-      "prevControl": {
-        "x": 5.509565134948722,
-        "y": 3.226444756275118
-      },
-      "nextControl": {
-        "x": 5.460371714516559,
-        "y": 4.902205073571968
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 4.735055812607463,
-        "y": 6.133043272882445
-      },
-      "prevControl": {
-        "x": 5.106674665068683,
-        "y": 6.1809940925605265
-      },
-      "nextControl": {
-        "x": 3.7000293453608792,
-        "y": 5.999491470641297
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.780072232928514,
-        "y": 6.011002645344107
-      },
-      "prevControl": {
-        "x": 3.5326180679689743,
-        "y": 5.975415621807755
-      },
-      "nextControl": {
-        "x": 4.027526397888053,
-        "y": 6.0465896688804595
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.7878323651372563,
-        "y": 6.008715381856753
-      },
-      "prevControl": {
-        "x": 3.539200549866918,
-        "y": 5.9825960320299245
-      },
-      "nextControl": {
-        "x": 4.036464180407595,
-        "y": 6.034834731683581
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.045238038417188,
-        "y": 7.650092201562578
-      },
-      "prevControl": {
-        "x": 1.7991783016715992,
-        "y": 7.6058812758462855
-      },
-      "nextControl": {
-        "x": 2.2912977751627763,
-        "y": 7.694303127278871
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 0.9744459885853738,
-        "y": 4.6167229448664475
-      },
-      "prevControl": {
-        "x": 0.7248159400374581,
-        "y": 4.630318490532778
-      },
-      "nextControl": {
-        "x": 1.2240760371332904,
-        "y": 4.603127399200117
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.833662245012324,
-        "y": 2.853709352937069
-      },
-      "prevControl": {
-        "x": 5.814051418158163,
-        "y": 3.102938996980663
-      },
-      "nextControl": {
-        "x": 5.853273071866484,
-        "y": 2.604479708893476
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.345091538394257,
-        "y": 5.195327906283456
-      },
-      "prevControl": {
-        "x": 3.7842404867504196,
-        "y": 5.213024584543108
-      },
-      "nextControl": {
-        "x": 2.35508229182681,
-        "y": 5.155432842942953
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.967506839071151,
-        "y": 2.853709352937069
-      },
-      "prevControl": {
-        "x": 3.742412623667063,
-        "y": 4.167081416357846
-      },
-      "nextControl": {
-        "x": 2.192601054475239,
-        "y": 1.5403372895162928
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 0.0,
-        "y": 0.0
-      },
-      "prevControl": {
-        "x": 0.45553278688744697,
-        "y": 0.9501536885305403
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 1.57,
-      "rotationDegrees": 180.0
-    }
-  ],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 5bf438c..b586433 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -1,32 +1,41 @@
 {
-  "robotWidth": 0.9,
-  "robotLength": 0.9,
+  "robotWidth": 0.826,
+  "robotLength": 0.8255,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "Hang Left",
+    "Neutral Left",
+    "Neutral Right",
+    "Alliance Center"
+  ],
   "autoFolders": [],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
+  "defaultMaxAngVel": 360.0,
+  "defaultMaxAngAccel": 540.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 74.088,
-  "robotMOI": 6.883,
+  "robotMass": 51.94,
+  "robotMOI": 6.779055,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.048,
-  "driveGearing": 5.143,
-  "maxDriveSpeed": 5.45,
+  "driveGearing": 6.122448979591837,
+  "maxDriveSpeed": 5.04,
   "driveMotorType": "krakenX60",
   "driveCurrentLimit": 60.0,
   "wheelCOF": 1.2,
-  "flModuleX": 0.273,
-  "flModuleY": 0.273,
-  "frModuleX": 0.273,
-  "frModuleY": -0.273,
-  "blModuleX": -0.273,
-  "blModuleY": 0.273,
-  "brModuleX": -0.273,
-  "brModuleY": -0.273,
+  "flModuleX": 0.276225,
+  "flModuleY": 0.276225,
+  "frModuleX": 0.276225,
+  "frModuleY": -0.276225,
+  "blModuleX": -0.276225,
+  "blModuleY": 0.276225,
+  "brModuleX": -0.276225,
+  "brModuleY": -0.276225,
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
-  "robotFeatures": []
+  "robotFeatures": [
+    "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.57,\"y\":0.0},\"size\":{\"width\":0.8,\"length\":0.3},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}",
+    "{\"name\":\"Line\",\"type\":\"line\",\"data\":{\"start\":{\"x\":0.343,\"y\":0.0},\"end\":{\"x\":1.53,\"y\":2.77},\"strokeWidth\":0.02}}",
+    "{\"name\":\"Line\",\"type\":\"line\",\"data\":{\"start\":{\"x\":0.343,\"y\":0.0},\"end\":{\"x\":1.53,\"y\":-2.77},\"strokeWidth\":0.02}}"
+  ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
new file mode 100644
index 0000000..fccf68e
--- /dev/null
+++ b/src/main/java/frc/robot/BuildConstants.java
@@ -0,0 +1,19 @@
+package frc.robot;
+
+/**
+ * Automatically generated file containing build version information.
+ */
+public final class BuildConstants {
+  public static final String MAVEN_GROUP = "";
+  public static final String MAVEN_NAME = "Robot2026";
+  public static final String VERSION = "unspecified";
+  public static final int GIT_REVISION = 184;
+  public static final String GIT_SHA = "80d73638a95ef8d42e2e46c6d04b67b1725ed703";
+  public static final String GIT_DATE = "2026-03-20 03:13:32 EDT";
+  public static final String GIT_BRANCH = "sotm";
+  public static final String BUILD_DATE = "2026-03-22 13:47:41 EDT";
+  public static final long BUILD_UNIX_TIME = 1774201661432L;
+  public static final int DIRTY = 1;
+
+  private BuildConstants(){}
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f9fac0a..e61b2af 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -3,7 +3,12 @@
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.path.PathConstraints;
 
+import edu.wpi.first.math.geometry.*;
+
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.lib.field.FieldLayout;
+import frc.robot.lib.field.FieldUtil;
 import frc.robot.subsystems.drive.DriveConstants;
 
 /**
@@ -21,10 +26,6 @@ public static final class Controllers {
 		public static final double DRIVER_DEADBAND = 0.07;
 	}
 
-	public static final class Odometry {
-	}
-	
-
 	public static final class Pathplanner {
 		public static RobotConfig config;
 		static {
@@ -34,11 +35,315 @@ public static final class Pathplanner {
 				DriverStation.reportError("Pathplanner configs failed to load ", e.getStackTrace());
 			}
 		}
-		public static final PathConstraints GLOBAL_CONSTRAINTS = 
-			new PathConstraints(DriveConstants.MAX_SPEED * 0.85, 
-				DriveConstants.MAX_ACCEL * 0.85, 
-				DriveConstants.MAX_ROTATION_SPEED * 0.85, 
+		public static final PathConstraints GLOBAL_CONSTRAINTS = new PathConstraints(DriveConstants.MAX_SPEED * 0.85,
+				DriveConstants.MAX_ACCEL * 0.85,
+				DriveConstants.MAX_ROTATION_SPEED * 0.85,
 				DriveConstants.MAX_ROTATION_ACCEL * 0.85);
 		public static final double GENERATION_WAIT_TIME = 5;
 	}
+
+	// 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.
+	/**
+	 * 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 static class FieldConstants {
+		public static Translation3d allianceCorrected(Translation3d t) {
+			if (DriverStation.getAlliance().isPresent()) {
+				Alliance a = DriverStation.getAlliance().orElseThrow();
+				if (a == Alliance.Red) {
+					var t2d = FieldUtil.flipTranslation(t.toTranslation2d());
+					return new Translation3d(t2d.getX(), t2d.getY(), t.getZ());
+				} else {
+					return t;
+				}
+			} else {
+				return t;
+			}
+		}
+
+		// AprilTag related constants
+		public static final int aprilTagCount = FieldLayout.APRILTAG_MAP.getTags().size();
+		public static final double aprilTagWidth = edu.wpi.first.math.util.Units.inchesToMeters(6.5);
+
+		// Field dimensions
+		public static final double fieldLength = FieldLayout.APRILTAG_MAP.getFieldLength();
+		public static final double fieldWidth = FieldLayout.APRILTAG_MAP.getFieldWidth();
+
+		// Fuel dimensions
+		public static final double fuelDiameter = edu.wpi.first.math.util.Units.inchesToMeters(5.91);
+
+		/**
+		 * 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 = FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX();
+			public static final double allianceZone = starting;
+			public static final double hubCenter = FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX()
+					+ Hub.width / 2.0;
+			public static final double neutralZoneNear = center - edu.wpi.first.math.util.Units.inchesToMeters(120);
+			public static final double neutralZoneFar = center + edu.wpi.first.math.util.Units.inchesToMeters(120);
+			public static final double oppHubCenter = FieldLayout.APRILTAG_MAP.getTagPose(4).get().getX()
+					+ Hub.width / 2.0;
+			public static final double oppAllianceZone = FieldLayout.APRILTAG_MAP.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 rightBumpMiddle = (rightBumpStart + rightBumpEnd) / 2.0;
+			public static final double rightTrenchOpenStart = rightBumpEnd
+					- edu.wpi.first.math.util.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 leftBumpMiddle = (leftBumpStart + leftBumpEnd) / 2.0;
+			public static final double leftTrenchOpenEnd = leftBumpStart
+					+ edu.wpi.first.math.util.Units.inchesToMeters(12.0);
+			public static final double leftTrenchOpenStart = fieldWidth;
+		}
+
+		/** Hub related constants */
+		public static class Hub {
+			// Dimensions
+			public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(46.0);
+			public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(72.0); // includes the
+																									// catcher at the
+																									// top
+			public static final double innerWidth = edu.wpi.first.math.util.Units.inchesToMeters(41.7);
+			public static final double innerHeight = edu.wpi.first.math.util.Units.inchesToMeters(56.5);
+
+			// Relevant reference points on alliance side
+			public static final Translation3d topCenterPoint = new Translation3d(
+					FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX() + width / 2.0,
+					fieldWidth / 2.0,
+					height);
+			public static final Translation3d innerCenterPoint = new Translation3d(
+					FieldLayout.APRILTAG_MAP.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(
+					FieldLayout.APRILTAG_MAP.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 = FieldLayout.APRILTAG_MAP.getTagPose(26).get().toPose2d();
+			public static final Pose2d farFace = FieldLayout.APRILTAG_MAP.getTagPose(20).get().toPose2d();
+			public static final Pose2d rightFace = FieldLayout.APRILTAG_MAP.getTagPose(18).get().toPose2d();
+			public static final Pose2d leftFace = FieldLayout.APRILTAG_MAP.getTagPose(21).get().toPose2d();
+		}
+
+		/** Left Bump related constants */
+		public static class LeftBump {
+
+			// Dimensions
+			public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(73.0);
+			public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(6.513);
+			public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(44.4);
+
+			// Relevant reference points on alliance side
+			public static final Translation2d nearLeftCorner = new Translation2d(LinesVertical.hubCenter - width / 2,
+					edu.wpi.first.math.util.Units.inchesToMeters(255));
+			public static final Translation2d nearRightCorner = Hub.nearLeftCorner;
+			public static final Translation2d farLeftCorner = new Translation2d(LinesVertical.hubCenter + width / 2,
+					edu.wpi.first.math.util.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,
+					edu.wpi.first.math.util.Units.inchesToMeters(255));
+			public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner;
+			public static final Translation2d oppFarLeftCorner = new Translation2d(LinesVertical.hubCenter + width / 2,
+					edu.wpi.first.math.util.Units.inchesToMeters(255));
+			public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner;
+		}
+
+		/** Right Bump related constants */
+		public static class RightBump {
+			// Dimensions
+			public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(73.0);
+			public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(6.513);
+			public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(44.4);
+
+			// Relevant reference points on alliance side
+			public static final Translation2d nearLeftCorner = new Translation2d(LinesVertical.hubCenter + width / 2,
+					edu.wpi.first.math.util.Units.inchesToMeters(255));
+			public static final Translation2d nearRightCorner = Hub.nearLeftCorner;
+			public static final Translation2d farLeftCorner = new Translation2d(LinesVertical.hubCenter - width / 2,
+					edu.wpi.first.math.util.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,
+					edu.wpi.first.math.util.Units.inchesToMeters(255));
+			public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner;
+			public static final Translation2d oppFarLeftCorner = new Translation2d(LinesVertical.hubCenter - width / 2,
+					edu.wpi.first.math.util.Units.inchesToMeters(255));
+			public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner;
+		}
+
+		/** Left Trench related constants */
+		public static class LeftTrench {
+			// Dimensions
+			public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(65.65);
+			public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(47.0);
+			public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(40.25);
+			public static final double openingWidth = edu.wpi.first.math.util.Units.inchesToMeters(50.34);
+			public static final double openingHeight = edu.wpi.first.math.util.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 RightTrench {
+
+			// Dimensions
+			public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(65.65);
+			public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(47.0);
+			public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(40.25);
+			public static final double openingWidth = edu.wpi.first.math.util.Units.inchesToMeters(50.34);
+			public static final double openingHeight = edu.wpi.first.math.util.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 = edu.wpi.first.math.util.Units.inchesToMeters(49.25);
+			public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(45.0);
+			public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(78.25);
+			public static final double innerOpeningWidth = edu.wpi.first.math.util.Units.inchesToMeters(32.250);
+			public static final double frontFaceX = edu.wpi.first.math.util.Units.inchesToMeters(43.51);
+
+			public static final double uprightHeight = edu.wpi.first.math.util.Units.inchesToMeters(72.1);
+
+			// Rung heights from the floor
+			public static final double lowRungHeight = edu.wpi.first.math.util.Units.inchesToMeters(27.0);
+			public static final double midRungHeight = edu.wpi.first.math.util.Units.inchesToMeters(45.0);
+			public static final double highRungHeight = edu.wpi.first.math.util.Units.inchesToMeters(63.0);
+
+			// Relevant reference points on alliance side
+			public static final Translation2d centerPoint = new Translation2d(
+					frontFaceX, FieldLayout.APRILTAG_MAP.getTagPose(31).get().getY());
+			public static final Translation2d leftUpright = new Translation2d(
+					frontFaceX,
+					(FieldLayout.APRILTAG_MAP.getTagPose(31).get().getY())
+							+ innerOpeningWidth / 2
+							+ edu.wpi.first.math.util.Units.inchesToMeters(0.75));
+			public static final Translation2d rightUpright = new Translation2d(
+					frontFaceX,
+					(FieldLayout.APRILTAG_MAP.getTagPose(31).get().getY())
+							- innerOpeningWidth / 2
+							- edu.wpi.first.math.util.Units.inchesToMeters(0.75));
+
+			// Relevant reference points on opposing side
+			public static final Translation2d oppCenterPoint = new Translation2d(
+					fieldLength - frontFaceX,
+					FieldLayout.APRILTAG_MAP.getTagPose(15).get().getY());
+			public static final Translation2d oppLeftUpright = new Translation2d(
+					fieldLength - frontFaceX,
+					(FieldLayout.APRILTAG_MAP.getTagPose(15).get().getY())
+							+ innerOpeningWidth / 2
+							+ edu.wpi.first.math.util.Units.inchesToMeters(0.75));
+			public static final Translation2d oppRightUpright = new Translation2d(
+					fieldLength - frontFaceX,
+					(FieldLayout.APRILTAG_MAP.getTagPose(15).get().getY())
+							- innerOpeningWidth / 2
+							- edu.wpi.first.math.util.Units.inchesToMeters(0.75));
+		}
+
+		public static class Depot {
+			// Dimensions
+			public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(42.0);
+			public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(27.0);
+			public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(1.125);
+			public static final double distanceFromCenterY = edu.wpi.first.math.util.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 = edu.wpi.first.math.util.Units.inchesToMeters(31.8);
+			public static final double openingDistanceFromFloor = edu.wpi.first.math.util.Units.inchesToMeters(28.1);
+			public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(7.0);
+
+			// Relevant reference points on alliance side
+			public static final Translation2d centerPoint = new Translation2d(0,
+					FieldLayout.APRILTAG_MAP.getTagPose(29).get().getY());
+		}
+	}
 }
diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java
index 2dbdf00..db72991 100644
--- a/src/main/java/frc/robot/ControlsMapping.java
+++ b/src/main/java/frc/robot/ControlsMapping.java
@@ -1,24 +1,46 @@
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.MetersPerSecond;
 import static frc.robot.Robot.controller;
 
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.lib.field.FieldUtil;
+import frc.robot.subsystems.climb.Climb;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveConstants;
 import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.roller.Roller;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShotCalculator;
 import frc.robot.subsystems.vision.VisionDeviceManager;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ProxyCommand;
+
 
 public class ControlsMapping {
+
 	public static void mapTeleopCommand() {
 		Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl()));
 		// run sysID functions
 		Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER);
 		
 		controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d()));
+		controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true));
+		controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false));
+		controller.x().whileTrue(Drive.getInstance().autopilotAlign(true));
+		controller.y().whileTrue(Drive.getInstance().autopilotAlign(false));
+		new Trigger(() -> controller.getHID().getPOV() != -1).whileTrue(Drive.getInstance().nudgeCommand());
 
 		controller.b().whileTrue(Drive.getInstance().headingLockToPose(DriveConstants.FieldPoses.TAG.pose));
 		controller.x().onTrue(Drive.getInstance().pathFindToThisRandomPlaceIdk());
@@ -27,48 +49,108 @@ public static void mapTeleopCommand() {
 		// controller.x().whileTrue(Drive.getInstance().autopilotAlign(true));
 		// controller.y().whileTrue(Drive.getInstance().autopilotAlign(false));
 		controller.y().onTrue(VisionDeviceManager.getInstance().bootUp());
+		// Intake.getInstance().setDefaultCommand(Intake.getInstance().intake());
+
+		controller.back().onTrue(Drive.getInstance().resetPoseCommand(new
+			Pose2d()));
+		controller.y().onTrue(VisionDeviceManager.getInstance().bootUp());
+
+		// controller.leftTrigger().debounce(0.1).whileTrue(Intake.getInstance().outtake());
+		// controller.b().whileTrue(Intake.getInstance().outtake());
+		// controller.x().onTrue(Climb.getInstance().hangCommand());
+
+		// controller.rightTrigger().debounce(0.1
+		// // ).onTrue(
+		// // Commands.parallel(
+		// // Shooter.getLeftInstance().shoot(50, -50),
+		// // Shooter.getRightInstance().shoot(50, -50))
+		// // ).onFalse(
+		// // Commands.parallel(
+		// // Shooter.getLeftInstance().stop(),
+		// // Shooter.getRightInstance().stop())
+		// ).whileTrue(
+		// Commands.parallel(
+		// Indexer.getLeftInstance().activateIndexer(),
+		// Indexer.getRightInstance().activateIndexer())
+		// ).whileTrue(
+		// Commands.repeatingSequence(
+		// Commands.runOnce(() -> Robot.fuelSim.launchFuel(
+		// MetersPerSecond.of(
+		// Shooter.getLeftInstance().getTopSpeed() * Constants.TAU * 0.0508),
+		// Degrees.of(75),
+		// Degrees.of(0),
+		// Inches.of(19)
+		// )).andThen(Commands.waitSeconds(0.1))
+		// )
+		// );
+		controller.a().whileTrue(
+			Commands.parallel(
+				Shooter.getRightInstance().shoot(60, 60),
+				Shooter.getLeftInstance().shoot(60, 60)
+			)
+		).onFalse(
+			Commands.parallel(
+				Shooter.getRightInstance().stop(),
+				Shooter.getLeftInstance().stop()
+			)
+		);
+
+		controller.x().whileTrue(
+			Commands.parallel(
+				Indexer.getRightInstance().activateIndexer(),
+				Indexer.getLeftInstance().activateIndexer(),
+				Roller.getInstance().roll()
+			)
+		).onFalse(
+			Commands.parallel(
+				Indexer.getRightInstance().deactivateIndexer(),
+				Indexer.getLeftInstance().deactivateIndexer(),
+				Roller.getInstance().stop()
+			)
+		);
+
+
+		controller.b().whileTrue(
+				Intake.getInstance().intake()).onFalse(Intake.getInstance().stow());
+		// controller.y().onTrue(Intake.getInstance().)
+		controller.leftBumper().whileTrue(Drive.getInstance().headingLockToHub());
+		// Shooter.getRightInstance().shoot()));
 	}
 
 	public static void mapSysId() {
 		// set up sysID routine type
-		controller.a().onTrue(Commands.runOnce(
-			() -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION)));
-		controller.b().onTrue(Commands.runOnce(
-			() -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION)));
-		controller.back().onTrue(Commands.runOnce(
-			() -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER)));
+		controller.a().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION)));
+		controller.b().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION)));
+		controller.back().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER)));
 		// map the sysid routine movement directions
-		controller.leftBumper().and(controller.x()).whileTrue(
-			Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward)
-				.finallyDo((
-					boolean interrupted) -> {
-						if (interrupted) {
-							Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle());
-						}
-					}));
-		controller.leftBumper().and(controller.x()).whileTrue(
-			Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse)
-				.finallyDo((
-					boolean interrupted) -> {
-						if (interrupted) {
-							Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle());
-						}
-					}));
-		controller.rightBumper().and(controller.x()).whileTrue(
-			Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward)
-				.finallyDo((
-					boolean interrupted) -> {
-						if (interrupted) {
-							Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle());
-						}
-					}));
-		controller.rightBumper().and(controller.y()).whileTrue(
-			Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse)
-				.finallyDo((
-					boolean interrupted) -> {
-						if (interrupted) {
-							Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle());
-						}
-					}));
+		controller.leftBumper().and(controller.x())
+			.whileTrue(
+				new ProxyCommand(
+					()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward)
+						.finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle()))
+				)
+			);
+		controller.leftBumper().and(controller.y())
+			.whileTrue(
+				new ProxyCommand(
+					()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse)
+						.finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle()))
+				)
+			);
+		controller.rightBumper().and(controller.x())
+				.whileTrue(
+					new ProxyCommand(
+						()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward)
+							.finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle()))
+					)
+				);
+		controller.rightBumper().and(controller.y())
+				.whileTrue(
+					new ProxyCommand(
+						()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse)
+							.finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle()))
+					)
+				);
 	}
-}
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 9430949..ce89a20 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -1,26 +1,45 @@
 package frc.robot;
 
 import frc.robot.auto.AutoSelector;
+import frc.robot.lib.sim.FuelSim;
+import frc.robot.lib.util.MovingAverageDouble;
+
+import static edu.wpi.first.units.Units.Inches;
 
 import java.util.Optional;
 
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+
 import com.pathplanner.lib.commands.FollowPathCommand;
 
-import edu.wpi.first.epilogue.Logged;
-import edu.wpi.first.epilogue.logging.EpilogueBackend;
-import edu.wpi.first.hal.AllianceStationID;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
-import edu.wpi.first.wpilibj2.command.*;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.Controllers;
+import frc.robot.auto.AutoSelector;
 import frc.robot.subsystems.TelemetryManager;
+import frc.robot.subsystems.climb.Climb;
 import frc.robot.subsystems.drive.*;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.led.Led;
+import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.vision.VisionDeviceManager;
 
 /**
@@ -29,13 +48,19 @@
  * this project, you must also update the Main.java file in the project.
  */
 @SuppressWarnings("unused")
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
 	private static final CommandScheduler commandScheduler = CommandScheduler.getInstance();
 	private AutoSelector autoChooser;
 	private Command autoCommand;
+	private static final String standardMap = "standard";
+    private static final String mapTwo = "mapTwo";
+    private final SendableChooser<String> mapChooser = new SendableChooser<>();
 
 	public static final CommandXboxController controller =
 		new CommandXboxController(Controllers.DRIVER_CONTROLLER_PORT);
+
+	public double lastTime = -1.0;
+	public MovingAverageDouble fpsTracker = new MovingAverageDouble(60);
 		
 	/**
 	 * This function is run when the robot is first started up and should be used for any
@@ -50,18 +75,77 @@ public Robot() {
 		commandScheduler.schedule(VisionDeviceManager.getInstance().bootUp());
 		autoChooser = new AutoSelector();
 
+		boolean replay = Logger.hasReplaySource();
 		//robot data loggers 
 		boolean usbPresent = new java.io.File("/u").exists();
 		if (usbPresent) {
-		  DataLogManager.start("/u/logs");  // USB stick
+		//   DataLogManager.start("/u/logs");  // USB stick
 		  System.out.println("Log/USB mounts OK");
 		} else {
-		  DataLogManager.start();           // falls back to /home/lvuser/logs
+		//   DataLogManager.start();           // falls back to /home/lvuser/logs
 		  System.out.println("Log/USB mounts NOT OK");
 		}
+
+		Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+        Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+        Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+        Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+        Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+        switch (BuildConstants.DIRTY) {
+            case 0:
+                Logger.recordMetadata("GitDirty", "All changes committed");
+                break;
+            case 1:
+                Logger.recordMetadata("GitDirty", "Uncomitted changes");
+                break;
+            default:
+                Logger.recordMetadata("GitDirty", "Unknown");
+                break;
+        }
+
+        if (RobotBase.isReal()) {
+            Logger.addDataReceiver(new WPILOGWriter());
+            if (!DriverStation.isFMSAttached()) {
+                Logger.addDataReceiver(new NT4Publisher());
+            }
+        } else if (replay) {
+            setUseTiming(false);
+            String logPath = LogFileUtil.findReplayLog();
+            Logger.setReplaySource(new WPILOGReader(logPath));
+            Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+        } else if (RobotBase.isSimulation()) {
+            Logger.addDataReceiver(new NT4Publisher());
+            Logger.addDataReceiver(new WPILOGWriter());
+        }
+
+        // Logger.start();
+        if (!Logger.hasReplaySource()) {
+            RobotController.setTimeSource(RobotController::getFPGATime);
+        }
+
+		// VisionDeviceManager.getInstance();
+		
+		// Drive.getInstance();
+		// Shooter.getLeftInstance();
+		// Shooter.getRightInstance();
+		// Indexer.getLeftInstance();
+		// Indexer.getRightInstance();
+		// Intake.getInstance();
+		// Climb.getInstance();
+
+		// TelemetryManager.getInstance();
+		// commandScheduler.schedule(FollowPathCommand.warmupCommand());
+		// commandScheduler.schedule(VisionDeviceManager.getInstance().bootUp());
+		// autoChooser = new AutoSelector();
+
+		Led.getInstance();
+		commandScheduler.schedule(
+			Led.getInstance().setRainbowCommand());
+
 		DriverStation.startDataLog(DataLogManager.getLog());
 	}
 
+
 	/**
 	 * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
 	 * that you want ran during disabled, autonomous, teleoperated and test.
@@ -75,7 +159,13 @@ public void robotPeriodic() {
 		// commands, running already-scheduled commands, removing finished or interrupted commands,
 		// and running subsystem periodic() methods.  This must be called from the robot's periodic
 		// block in order for anything in the Command-based framework to work.
+
 		commandScheduler.run();
+		double now = Timer.getFPGATimestamp();
+		fpsTracker.add(1.0 / (now - lastTime));
+		Logger.recordOutput("FPS", fpsTracker.getAverage());
+		Logger.recordOutput("rawDtMs", 1000 * (now - lastTime));
+		lastTime = now;
 	}
 
 	/** This function is called once each time the robot enters Disabled mode. */
@@ -112,13 +202,11 @@ public void autonomousExit() {
 
 	@Override
 	public void teleopInit() {
+		// ControlsMapping.mapTeleopCommand();
 		// This makes sure that the autonomous stops running when teleop starts running. 
 		if (autoCommand != null) {
 			autoCommand.cancel();
 		}
-		Drive.getInstance().setDefaultCommand(Drive.getInstance().openLoopControl());
-		
-		ControlsMapping.mapTeleopCommand();
 	}
 
 	/** This function is called periodically during operator control. */
@@ -132,7 +220,7 @@ public void testInit() {
 		CommandScheduler.getInstance().cancelAll();
 
 		//map test commands
-		ControlsMapping.mapSysId();
+		// ControlsMapping.mapSysId();
 	}
 
 	/** This function is called periodically during test mode. */
@@ -140,13 +228,38 @@ public void testInit() {
 	public void testPeriodic() {
 	}
 
+	public static FuelSim fuelSim;
+
 	/** This function is called once when the robot is first started up. */
 	@Override
 	public void simulationInit() {
+		fuelSim = new FuelSim();
+		// fuelSim.spawnStartingFuel();
+		fuelSim.registerRobot(
+			Inches.of(27),
+			Inches.of(27),
+			Inches.of(0.5),
+			() -> Drive.getInstance().getPose(),
+			() -> Drive.getInstance().getFieldSpeeds());
+		fuelSim.registerIntake(
+				Inches.of(-13), Inches.of(13), Inches.of(-21.5), Inches.of(-17.5));
+		fuelSim.start();
+		fuelSim.enableAirResistance();
+		timer.start();;
+
 	}
 
+	Timer timer = new Timer();
 	/** This function is called periodically whilst in simulation. */
 	@Override
 	public void simulationPeriodic() {
+		fuelSim.updateSim();
+		Logger.recordOutput("Blue Score", FuelSim.Hub.BLUE_HUB.getScore());
+		Logger.recordOutput("Red Score", FuelSim.Hub.RED_HUB.getScore());
+
+		if (timer.hasElapsed(5)) {
+			fuelSim.clearFuel();
+			timer.restart();
+		}
 	}
 }
diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java
index fcdc417..4f5a343 100644
--- a/src/main/java/frc/robot/auto/AutoRoutines.java
+++ b/src/main/java/frc/robot/auto/AutoRoutines.java
@@ -4,25 +4,281 @@
 import frc.robot.lib.trajectory.RedTrajectory;
 import frc.robot.lib.trajectory.TrajectoryLoader;
 import frc.robot.lib.trajectory.RedTrajectory.TrajectoryType;
+import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.commands.PIDToPoseCommand;
 import frc.robot.subsystems.drive.commands.TrajectoryCommand;
+import frc.robot.subsystems.intake.Intake;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
 
 public final class AutoRoutines {
-	@Auto(name = "Pid Test")
+	// @Auto(name = "Pid Test")
 	public static Command testPidToPose() {
 		return new PIDToPoseCommand(
 			new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120)));
 	}
 
-	@Auto(name = "Trajectory Test")
+	// @Auto(name = "Trajectory Test")
 	public static Command testTrajectoryAuto() {
 		RedTrajectory traj = TrajectoryLoader.loadAutoTrajectory(
 			TrajectoryType.CHOREO, 
 			"testPath3").get();
 		return new TrajectoryCommand(traj);
 	}
+
+	@Auto(name = "right neutral auto")
+	public static Command rightAutoNeutral() {
+		var tTrenchRight = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"TrenchRight");
+
+		if (tTrenchRight.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var tSwipeRight = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"SwipeRight");
+
+		if (tSwipeRight.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var tReturnTrenchRight = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"ReturnTrenchRight");
+
+		if (tReturnTrenchRight.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var crossTrench = tTrenchRight.get();
+		var swipe = tSwipeRight.get();
+		var back = tReturnTrenchRight.get();
+		return Commands.print(Timer.getFPGATimestamp() + ": Time start")
+			.andThen(Intake.getInstance().calibrateZero())
+				.alongWith(
+					new TrajectoryCommand(crossTrench))
+			.andThen(
+				Intake.getInstance().intake())
+			.andThen(
+				new TrajectoryCommand(swipe))
+			.andThen(
+				Intake.getInstance().lower()
+					.alongWith(
+						new TrajectoryCommand(back)))
+			.andThen(
+				Drive.getInstance().headingLockToHub()
+					.raceWith(
+						Automation.shootAll()
+							.andThen(
+								Commands.waitSeconds(0.5))
+							.andThen(
+								Automation.indexAll())
+							.andThen(
+								Commands.waitSeconds(3))
+							.andThen(
+								Intake.getInstance().stow())
+							.andThen(
+								Commands.waitSeconds(3))))
+			// .andThen(
+			// 	new AutopilotCommand(
+			// 		new APTarget(
+			// 			new Pose2d(Constants.FieldConstants.Tower.rightUpright, Rotation2d.kCCW_90deg))
+			// 			.withEntryAngle(Rotation2d.kZero)))
+			.andThen(
+				Commands.print(Timer.getFPGATimestamp() + ": Time end"),
+				Commands.idle())
+			.finallyDo(
+				() -> {
+					CommandScheduler.getInstance().schedule(
+						Drive.getInstance().openLoopControl(),
+						Intake.getInstance().lower(),
+						Automation.stopShoot(),
+						Automation.stopIndex());
+				});
+	}
+
+	@Auto(name = "left neutral auto")
+	public static Command leftAutoNeutral() {
+		var tTrenchLeft = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"TrenchLeft");
+
+		if (tTrenchLeft.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var tSwipeLeft = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"SwipeLeft");
+
+		if (tSwipeLeft.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var tReturnTrenchLeft = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"ReturnTrenchLeft");
+
+		if (tReturnTrenchLeft.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var crossTrench = tTrenchLeft.get();
+		var swipe = tSwipeLeft.get();
+		var back = tReturnTrenchLeft.get();
+		return Commands.print(Timer.getFPGATimestamp() + ": Time start")
+			.andThen(Intake.getInstance().calibrateZero())
+			.alongWith(
+				new TrajectoryCommand(crossTrench))
+			.andThen(
+				Intake.getInstance().intake())
+			.andThen(
+				new TrajectoryCommand(swipe))
+			.andThen(
+				Intake.getInstance().lower()
+					.alongWith(
+						new TrajectoryCommand(back)))
+			.andThen(
+				Drive.getInstance().headingLockToHub()
+					.alongWith(
+						Automation.shootAll()
+							.andThen(
+								Commands.waitSeconds(0.5))
+							.andThen(
+								Automation.indexAll()
+									.andThen(
+										Commands.waitSeconds(3))
+									.andThen(
+										Intake.getInstance().stow()))))
+			.andThen(
+				Commands.print(Timer.getFPGATimestamp() + ": Time end"),
+				Commands.idle())
+			.finallyDo(
+				() -> {
+					CommandScheduler.getInstance().schedule(
+						Drive.getInstance().openLoopControl(),
+						Intake.getInstance().lower(),
+						Automation.stopShoot(),
+						Automation.stopIndex());
+				});
+	}
+
+	@Auto(name = "center auto")
+	public static Command centerAuto() {
+		var tDepotCenter = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"DepotCenter");
+
+		if (tDepotCenter.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var tDepotShootCenter = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"DepotShootCenter");
+
+		if (tDepotShootCenter.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var tStationCenter = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"StationCenter");
+
+		if (tStationCenter.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var tStationShootCenter = TrajectoryLoader.loadAutoTrajectory(
+			TrajectoryType.PATHPLANNER, 
+			"StationShootCenter");
+
+		if (tStationCenter.isEmpty()) {
+			DriverStation.reportWarning(
+				"Something happened", true);
+			return Commands.none();
+		}
+
+		var depotCenter = tDepotCenter.get();
+		var depotShootCenter = tDepotShootCenter.get();
+		var stationCenter = tStationCenter.get();
+		var stationShootCenter = tStationShootCenter.get();
+
+		return Commands.print(Timer.getFPGATimestamp() + ": Time start")
+			.andThen(Intake.getInstance().calibrateZero())
+			.andThen(Intake.getInstance().intake())
+			.alongWith(new TrajectoryCommand(depotCenter))
+			.andThen(
+				new TrajectoryCommand(depotShootCenter))
+			.andThen(
+				Drive.getInstance().headingLockToHub()
+					.raceWith(
+						Automation.shootAll()
+							.andThen(
+								Commands.waitSeconds(0.5))
+							.andThen(
+								Automation.indexAll())
+							.andThen(
+								Commands.waitSeconds(3))))
+			.andThen(
+				Intake.getInstance().lower(),
+				Automation.stopShoot(),
+				Automation.stopIndex())
+			.andThen(
+				new TrajectoryCommand(stationCenter))
+			.andThen(
+				Commands.waitSeconds(3))
+			.andThen(
+				new TrajectoryCommand(stationShootCenter))
+			.andThen(
+				Drive.getInstance().headingLockToHub()
+					.alongWith(
+						Automation.shootAll()
+							.andThen(
+								Commands.waitSeconds(0.5))
+							.andThen(
+								Automation.indexAll())
+							.andThen(
+								Commands.waitSeconds(3))
+							.andThen(
+								Intake.getInstance().stow())))
+			.andThen(
+				Commands.print(Timer.getFPGATimestamp() + ": Time end"),
+				Commands.idle())
+			.finallyDo(
+				() -> {
+					CommandScheduler.getInstance().schedule(
+						Drive.getInstance().openLoopControl(),
+						Intake.getInstance().lower(),
+						Automation.stopShoot(),
+						Automation.stopIndex());
+				});
+	}
 }
 
diff --git a/src/main/java/frc/robot/auto/AutoSelector.java b/src/main/java/frc/robot/auto/AutoSelector.java
index 4cb4551..f2c2d16 100644
--- a/src/main/java/frc/robot/auto/AutoSelector.java
+++ b/src/main/java/frc/robot/auto/AutoSelector.java
@@ -5,12 +5,14 @@
 import java.lang.annotation.RetentionPolicy;
 import java.lang.annotation.Target;
 import java.lang.reflect.Method;
+import java.util.Optional;
 import java.util.function.Supplier;
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 
 /**
  * A class to select autos
@@ -40,6 +42,8 @@ public AutoSelector() {
                         try {
                             return (Command) auto.invoke(null);
                         } catch (Exception e) {
+                            DriverStation.reportWarning(
+                                "something really bad happened " + e.getMessage(), true);
                             return null;
                         }
                     });
@@ -49,13 +53,14 @@ public AutoSelector() {
                 }
             }
         }
+        // chooser.addOption("right", () -> AutoRoutines.leftAutoNeutral());
 
         chooser.setDefaultOption("None", () -> null);
-        SmartDashboard.putData(chooser);
+        SmartDashboard.putData("Auto Selector", chooser);
     }
 
     /** Gets the auto selected from the SmartDashboard */
     public Command getAuto() {
-        return chooser.getSelected().get();
+        return Optional.of(chooser.getSelected()).orElse(() -> Commands.none()).get();
     }
 }
diff --git a/src/main/java/frc/robot/auto/Automation.java b/src/main/java/frc/robot/auto/Automation.java
index 8de0739..9a97149 100644
--- a/src/main/java/frc/robot/auto/Automation.java
+++ b/src/main/java/frc/robot/auto/Automation.java
@@ -1,5 +1,42 @@
 package frc.robot.auto;
 
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.roller.Roller;
+import frc.robot.subsystems.shooter.Shooter;
+
 public class Automation {
-    
+	public static Command shootAll() {
+		return Commands.parallel(
+			Shooter.getRightInstance().shoot(),
+			Shooter.getLeftInstance().shoot());
+	}
+
+	public static Command stopShoot() {
+		return Commands.parallel(
+			Shooter.getRightInstance().stop(),
+			Shooter.getLeftInstance().stop());
+	}
+
+	public static Command indexAll() {
+		return Commands.parallel(
+			Indexer.getRightInstance().activateIndexer(),
+			Indexer.getLeftInstance().activateIndexer(),
+			Roller.getInstance().roll());
+	}
+
+	public static Command stopIndex() {
+		return Commands.parallel(
+			Indexer.getRightInstance().deactivateIndexer(),
+			Indexer.getLeftInstance().deactivateIndexer(),
+			Roller.getInstance().stop());
+	}
+
+	public static Command backIndex() {
+		return Commands.parallel(
+				Indexer.getRightInstance().back(),
+				Indexer.getLeftInstance().back(),
+				Roller.getInstance().antiRoll());
+	}
 }
diff --git a/src/main/java/frc/robot/lib/field/FieldLayout.java b/src/main/java/frc/robot/lib/field/FieldLayout.java
index 4eb487e..ef6fa3e 100644
--- a/src/main/java/frc/robot/lib/field/FieldLayout.java
+++ b/src/main/java/frc/robot/lib/field/FieldLayout.java
@@ -36,8 +36,8 @@
 public class FieldLayout {
 	//TODO: this must be tuned to the specific year's field
 	public static Field2d field;
-	public static final double FIELD_LENGTH = Units.inchesToMeters(651.223);
-	public static final double FIELD_WIDTH = Units.inchesToMeters(323.277);
+	public static final double FIELD_LENGTH;
+	public static final double FIELD_WIDTH;
 
 	public static final double APRITAG_WIDTH = Units.inchesToMeters(6.50);
 	public static AprilTagFieldLayout APRILTAG_MAP;
@@ -54,6 +54,9 @@ public class FieldLayout {
 			DriverStation.reportError(e.getMessage(), false);
 			APRILTAG_MAP = AprilTagLayoutGenerated.getLayout();
 		}
+
+		FIELD_LENGTH = APRILTAG_MAP.getFieldLength();
+		FIELD_WIDTH = APRILTAG_MAP.getFieldWidth();
 		// APRILTAG_MAP = AprilTagLayoutGenerated.getLayout();
 		field = new Field2d();
 		SmartDashboard.putData(field);
diff --git a/src/main/java/frc/robot/lib/houndlib/BallConstants.java b/src/main/java/frc/robot/lib/houndlib/BallConstants.java
new file mode 100644
index 0000000..5f703fc
--- /dev/null
+++ b/src/main/java/frc/robot/lib/houndlib/BallConstants.java
@@ -0,0 +1,38 @@
+package frc.robot.lib.houndlib;
+
+public class BallConstants {
+    public final double mass;
+    public final double radius;
+    public final double area;
+
+    public final double rho;
+    public final double cd;
+    public final double clGain;
+    public final double clMax;
+
+    public final double gravity;
+    public final double spinDecayTau;
+
+    public BallConstants(
+            double mass,
+            double radius,
+            double rho,
+            double cd,
+            double clGain,
+            double clMax,
+            double gravity,
+            double spinDecayTau) {
+
+        this.mass = mass;
+        this.radius = radius;
+        this.area = Math.PI * radius * radius;
+
+        this.rho = rho;
+        this.cd = cd;
+        this.clGain = clGain;
+        this.clMax = clMax;
+
+        this.gravity = gravity;
+        this.spinDecayTau = spinDecayTau;
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/lib/houndlib/BallPhysics.java b/src/main/java/frc/robot/lib/houndlib/BallPhysics.java
new file mode 100644
index 0000000..d29d843
--- /dev/null
+++ b/src/main/java/frc/robot/lib/houndlib/BallPhysics.java
@@ -0,0 +1,174 @@
+package frc.robot.lib.houndlib;
+
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+
+public final class BallPhysics {
+    public static final double GRAVITY = 9.81;
+
+    public record ShotSolution(
+            double launchPitchRad,
+            double launchSpeed,
+            double flightTimeSeconds) {
+    }
+
+    private BallPhysics() {
+    }
+
+    private static Translation3d gravityForce(BallConstants c) {
+        return new Translation3d(0, 0, -c.mass * c.gravity);
+    }
+
+    private static Translation3d dragForce(
+            Translation3d v, BallConstants c) {
+
+        double speed = v.getNorm();
+        if (speed < 1e-6)
+            return new Translation3d();
+
+        double scale = -0.5 * c.rho * c.cd * c.area * speed;
+        return v.times(scale);
+    }
+
+    private static Translation3d magnusForce(
+            Translation3d v, Translation3d omega, BallConstants c) {
+
+        double speed = v.getNorm();
+        double wMag = omega.getNorm();
+        if (speed < 1e-6 || wMag < 1e-6)
+            return new Translation3d();
+
+        double spinRatio = wMag * c.radius / speed;
+        double cl = Math.min(c.clGain * spinRatio, c.clMax);
+
+        Translation3d vHat = v.div(speed);
+        Translation3d wHat = omega.div(wMag);
+
+        Translation3d direction = new Translation3d(Vector.cross(wHat.toVector(), vHat.toVector()));
+        double magnitude = 0.5 * c.rho * cl * c.area * speed * speed;
+
+        return direction.times(magnitude);
+    }
+
+    private static Rotation3d integrateRotation(
+            Rotation3d current,
+            Translation3d omega,
+            double dt) {
+
+        Rotation3d delta = new Rotation3d(
+                omega.getX() * dt,
+                omega.getY() * dt,
+                omega.getZ() * dt);
+
+        return current.plus(delta);
+    }
+
+    public static void step(
+            BallState s, BallConstants c, double dt) {
+
+        Translation3d force = gravityForce(c)
+                .plus(dragForce(s.velocity, c))
+                .plus(magnusForce(s.velocity, s.omega, c));
+
+        Translation3d accel = force.div(c.mass);
+
+        double decay = Math.exp(-dt / c.spinDecayTau);
+        s.omega = s.omega.times(decay);
+
+        s.velocity = s.velocity.plus(accel.times(dt));
+
+        s.pose = new Pose3d(s.pose.getTranslation().plus(s.velocity.times(dt)),
+                integrateRotation(s.pose.getRotation(), s.omega, dt));
+    }
+
+    public static ShotSolution solveBallisticWithIncomingAngle(
+            Translation3d shooterPose,
+            Translation3d targetPose,
+            double incomingPitchRad) {
+
+        Translation3d s = shooterPose;
+        Translation3d t = targetPose;
+
+        double dx = t.getX() - s.getX();
+        double dy = t.getY() - s.getY();
+        double dz = t.getZ() - s.getZ();
+
+        double d = Math.hypot(dx, dy);
+        if (d < 1e-9) {
+            throw new IllegalArgumentException("Horizontal distance too small");
+        }
+
+        double tanThetaT = Math.tan(incomingPitchRad);
+
+        double rhs = dz - d * tanThetaT;
+        if (rhs <= 0) {
+            throw new IllegalArgumentException(
+                    "No physical solution: dz - d*tan(thetaT) must be > 0");
+        }
+
+        double T = Math.sqrt(2.0 * rhs / GRAVITY);
+
+        double vHoriz = d / T;
+        double vZ0 = vHoriz * tanThetaT + GRAVITY * T;
+
+        double launchSpeed = Math.hypot(vHoriz, vZ0);
+        double launchPitch = Math.atan2(vZ0, vHoriz);
+
+        return new ShotSolution(launchPitch, launchSpeed, T);
+    }
+
+    public static ShotSolution solveBallisticWithSpeed(
+            Translation3d shooterPose,
+            Translation3d targetPose,
+            double launchSpeed) {
+
+        Translation3d s = shooterPose;
+        Translation3d t = targetPose;
+
+        double dx = t.getX() - s.getX();
+        double dy = t.getY() - s.getY();
+        double dz = t.getZ() - s.getZ();
+
+        double d = Math.hypot(dx, dy);
+        if (d < 1e-9) {
+            throw new IllegalArgumentException("Horizontal distance too small");
+        }
+
+        double v2 = launchSpeed * launchSpeed;
+        double g = GRAVITY;
+
+        double discriminant = v2 * v2 - g * (g * d * d + 2.0 * dz * v2);
+        if (discriminant < 0) {
+            return new ShotSolution(0, 0, 0);
+        }
+
+        // LOW-ARC solution (use +Math.sqrt(...) for high arc)
+        double tanTheta = (v2 + Math.sqrt(discriminant)) / (g * d);
+
+        double launchPitch = Math.atan(tanTheta);
+
+        double vHoriz = launchSpeed * Math.cos(launchPitch);
+        double time = d / vHoriz;
+
+        return new ShotSolution(launchPitch, launchSpeed, time);
+    }
+
+    public static double minSpeedForAnyArc(
+            Translation3d shooterPose,
+            Translation3d targetPose) {
+
+        Translation3d s = shooterPose;
+        Translation3d t = targetPose;
+
+        double dx = t.getX() - s.getX();
+        double dy = t.getY() - s.getY();
+        double dz = t.getZ() - s.getZ();
+
+        double d = Math.hypot(dx, dy);
+
+        return Math.sqrt(
+                GRAVITY * (Math.hypot(d, dz) + dz));
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/lib/houndlib/BallState.java b/src/main/java/frc/robot/lib/houndlib/BallState.java
new file mode 100644
index 0000000..191ffde
--- /dev/null
+++ b/src/main/java/frc/robot/lib/houndlib/BallState.java
@@ -0,0 +1,19 @@
+package frc.robot.lib.houndlib;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Translation3d;
+
+public class BallState {
+    public Pose3d pose;
+    public Translation3d velocity;
+    public Translation3d omega; // rad/s
+
+    public BallState(
+            Pose3d position,
+            Translation3d velocity,
+            Translation3d omega) {
+        this.pose = position;
+        this.velocity = velocity;
+        this.omega = omega;
+    }
+}
diff --git a/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java b/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java
new file mode 100644
index 0000000..739668d
--- /dev/null
+++ b/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java
@@ -0,0 +1,268 @@
+package frc.robot.lib.houndlib;
+
+import java.util.function.Function;
+
+import frc.robot.lib.houndlib.BallPhysics.ShotSolution;
+import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.InterpolatingTreeMap;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+/**
+ * Provides static methods to calculate the effective target position to aim for
+ * when shooting on the fly.
+ */
+public class ShootOnTheFlyCalculator {
+    /**
+     * Calculates the time it will take for a projectile to reach a target given the
+     * robot's pose and the target's pose, and a function describing the
+     * projectile's velocity. This allows you to have a shooter that may shoot a
+     * projectile at varying speeds given varying distances from the targe.
+     * 
+     * @see #calculateEffectiveTargetLocation(Pose2d, Translation3d, ChassisSpeeds,
+     *      ChassisAccelerations, Function, double, double)
+     * 
+     * @param robotPose                      the current pose of the robot
+     * @param targetPose                     the 3D pose of the target. this should
+     *                                       be the center of the target, if that
+     *                                       makes sense (2024 game), but could also
+     *                                       be offset if desired. this may also be
+     *                                       deeper into the target area if required
+     *                                       (2020 game). this should take into
+     *                                       account any necessary field reflections
+     *                                       before being passed.
+     * @param xyDistanceToProjectileVelocity a function that takes in the
+     *                                       xy-distance from the robot to the goal,
+     *                                       and returns the velocity of the shot
+     *                                       projectile in m/s.
+     * @return the time it will take for the projectile to reach the target
+     */
+    public static double getTimeToShoot(Pose2d robotPose, Translation3d targetPose,
+            Function<Double, Double> xyDistanceToProjectileVelocity) {
+        Transform3d diff = 
+            new Pose3d(
+                new Translation3d(
+                    robotPose.getTranslation()
+                ), 
+                Rotation3d.kZero)
+            .minus(
+                new Pose3d(
+                    targetPose, 
+                    Rotation3d.kZero));
+        double xyDistance = new Translation2d(diff.getX(), diff.getY()).getNorm();
+        double distance = diff.getTranslation().getNorm();
+        double projectileVelocity = xyDistanceToProjectileVelocity.apply(xyDistance);
+        double time = distance / projectileVelocity;
+        return time;
+    }
+
+    public static double getTimeToShoot(
+            Translation3d shooterPose,
+            Translation3d targetPose,
+            double launchSpeed,
+            double launchPitchRad) {
+        Translation3d s = shooterPose;
+        Translation3d t = targetPose;
+
+        double dx = t.getX() - s.getX();
+        double dy = t.getY() - s.getY();
+
+        double horizontalDist = Math.hypot(dx, dy);
+        double vHoriz = launchSpeed * Math.cos(launchPitchRad);
+
+        if (vHoriz <= 1e-6) {
+            throw new IllegalArgumentException("Horizontal velocity too small");
+        }
+
+        return horizontalDist / vHoriz;
+    }
+
+    /**
+     * Calculates the effective position of the target given the position, velocity,
+     * and acceleration of the robot's chassis. Does not account for air resistance,
+     * though this is very often unnecessary.
+     * 
+     * <p>
+     * 
+     * When shooting a projectile while moving, the projectile inherits the
+     * translational velocity of the chassis. Shooting on the fly can be
+     * accomplished by targeting a "virtual" goal if you are moving, which acts to
+     * negate the forces applied on the projectile due to the movement of the
+     * chassis.
+     * 
+     * <p>
+     * 
+     * An iterative approach (see {@code goalPositionIterations}) is required
+     * because the time taken for the projectile to travel to the target will change
+     * given a different target location. To account for this, we re-simulate the
+     * projectile's travel with a new shot time derived from the new virtual goal
+     * position several times.
+     * 
+     * 
+     * <p>
+     * 
+     * When solving this problem mathematically, the acceleration of the chassis
+     * does not matter in the final velocities of the projectile (it will not
+     * inherit the acceleration of the chassis). The
+     * {@code accelerationCompensationFactor} is necessary due to other errors:
+     * 
+     * (1) the time taken to move the projectile through a shooter is non-zero, so
+     * (2) if the chassis is accelerating, the velocity of the chassis by the time
+     * the projectile leaves the robot will have changed.
+     * 
+     * To account for this, we multiply the acceleration at the time of the shot
+     * <i>command</i> by a specific value and add it to the velocity at the time of
+     * the shot. This value is based on the time delta between
+     * commanding a shot and the shot actually leaving the shooter, meaning that the
+     * effective velocity generated is the velocity as the projectile leaves the
+     * shooter. This is extremely complicated to determine theoretically, so if you
+     * find acceleration to be causing shot inaccuracies, find a value that provides
+     * adequate compensation (should be around [0,2]).
+     * 
+     * <p>
+     * 
+     * The easiest way to create the {@code xyDistanceToProjectileVelocity} lambda
+     * function is as follows:
+     * 
+     * If the speed of your shooter is always constant, simply create a lambda
+     * expression that always returns the same value.
+     * 
+     * If the speed of your shooter is controlled using an
+     * {@link InterpolatingTreeMap} based on distance from the goal, simply get the
+     * appropriate shooter speed from that map, and multiply it by some constant
+     * that describes how fast the projectile moves given a shooter speed. This can
+     * be calculated experimentally by pointing a camera at the shooter and
+     * calculating the speed of the projectile based on the distance travelled in n
+     * frames. If you find the relationship between shooter speed and projectile
+     * speed is not constant, you can create a second {@link InterpolatingTreeMap},
+     * or define it as an equation.
+     * 
+     * @param robotPose                      the current pose of the robot
+     * @param targetPose                     the 3D pose of the target. this should
+     *                                       be the center of the target, if that
+     *                                       makes sense (2024 game), but could also
+     *                                       be offset if desired. this may also be
+     *                                       deeper into the target area if required
+     *                                       (2020 game). this should take into
+     *                                       account any necessary field reflections
+     *                                       before being passed.
+     * @param fieldRelRobotVelocity          the field-relative velocity of the
+     *                                       robot's chassis
+     * @param fieldRelRobotAcceleration      the field-relative acceleration of the
+     *                                       robot's chassis
+     * @param xyDistanceToProjectileVelocity a function that takes in the
+     *                                       xy-distance from the robot to the goal,
+     *                                       and returns the velocity of the shot
+     *                                       projectile in m/s.
+     * @param goalPositionIterations         the number of iterations to use when
+     *                                       iteratively solving for the pose of the
+     *                                       target. a higher number of iterations
+     *                                       will increase the accuracy of the
+     *                                       result, but will also reduce
+     *                                       performance.
+     * @param accelerationCompensationFactor the value to multiply the acceleration
+     * @return
+     */
+    public static Translation3d calculateEffectiveTargetLocation(
+            Pose2d robotPose, Translation3d targetPose,
+            ChassisSpeeds fieldRelRobotVelocity,
+            ChassisAccels fieldRelRobotAcceleration,
+            Function<Double, Double> xyDistanceToProjectileVelocity,
+            double goalPositionIterations,
+            double accelerationCompensationFactor) {
+
+        double shotTime = getTimeToShoot(robotPose, targetPose, xyDistanceToProjectileVelocity);
+
+        Translation3d correctedTargetPose = new Translation3d();
+        for (int i = 0; i < goalPositionIterations; i++) {
+            double virtualGoalX = targetPose.getX()
+                    - shotTime * (fieldRelRobotVelocity.vxMetersPerSecond
+                            + fieldRelRobotAcceleration.ax
+                                    * accelerationCompensationFactor);
+            double virtualGoalY = targetPose.getY()
+                    - shotTime * (fieldRelRobotVelocity.vyMetersPerSecond
+                            + fieldRelRobotAcceleration.ay
+                                    * accelerationCompensationFactor);
+
+            correctedTargetPose = new Translation3d(virtualGoalX, virtualGoalY, targetPose.getZ());
+
+            double newShotTime = getTimeToShoot(robotPose, correctedTargetPose, xyDistanceToProjectileVelocity);
+
+            shotTime = newShotTime;
+            if (Math.abs(newShotTime - shotTime) <= 0.010) {
+                break;
+            }
+        }
+
+        return correctedTargetPose;
+    }
+
+    public record InterceptSolution(
+            Translation3d effectiveTargetPose,
+            double launchPitchRad,
+            double launchSpeed,
+            double flightTime,
+            double requiredYaw) {
+    }
+
+    public static InterceptSolution solveShootOnTheFly(
+            Translation3d shooterPose,
+            Translation3d targetPose,
+            ChassisSpeeds fieldRelRobotVelocity,
+            ChassisAccels fieldRelRobotAcceleration,
+            double incomingAngle,
+            int maxIterations,
+            double timeTolerance) {
+
+        ShotSolution sol = BallPhysics.solveBallisticWithSpeed(
+                shooterPose,
+                targetPose,
+                incomingAngle);
+
+        double t = sol.flightTimeSeconds();
+        Translation3d effectiveTarget = targetPose;
+
+        for (int i = 0; i < maxIterations; i++) {
+
+            double dx = fieldRelRobotVelocity.vxMetersPerSecond * t
+                + 0.5 * fieldRelRobotAcceleration.ax * t * t;
+
+            double dy = fieldRelRobotVelocity.vyMetersPerSecond * t
+                + 0.5 * fieldRelRobotAcceleration.ay * t * t;
+
+            effectiveTarget = new Translation3d(
+                    targetPose.getX() - dx,
+                    targetPose.getY() - dy,
+                    targetPose.getZ());
+
+            ShotSolution newSol = BallPhysics.solveBallisticWithIncomingAngle(
+                    shooterPose,
+                    effectiveTarget,
+                    incomingAngle);
+
+            if (Math.abs(newSol.flightTimeSeconds() - t) < timeTolerance) {
+                return new InterceptSolution(
+                        effectiveTarget,
+                        newSol.launchPitchRad(),
+                        newSol.launchSpeed(),
+                        newSol.flightTimeSeconds(),
+                        0);
+            }
+
+            sol = newSol;
+            t = newSol.flightTimeSeconds();
+        }
+
+        return new InterceptSolution(
+                effectiveTarget,
+                sol.launchPitchRad(),
+                sol.launchSpeed(),
+                sol.flightTimeSeconds(),
+                0);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/lib/io/CancoderIO.java b/src/main/java/frc/robot/lib/io/CancoderIO.java
new file mode 100644
index 0000000..4c73181
--- /dev/null
+++ b/src/main/java/frc/robot/lib/io/CancoderIO.java
@@ -0,0 +1,54 @@
+package frc.robot.lib.io;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.CANcoder;
+
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+
+public class CancoderIO {
+    @AutoLog
+    public static class CancoderIOInputs {
+        public int id = 0;
+        public boolean connected = false;
+        public double positionRotations = 0.0;
+        public double velocityRPS = 0.0;
+    }
+
+    public final String name;
+    public final CANcoder motor;
+    public final CancoderIOInputsAutoLogged inputs;
+
+    public StatusSignal<Angle> position;
+    public StatusSignal<AngularVelocity> velocity;
+            
+    private final Debouncer connectedDebouncer =
+        new Debouncer(0.5, Debouncer.DebounceType.kFalling);
+
+    public CancoderIO(String name, CANcoder motor) {
+        this.name = name;
+        this.motor = motor;
+        inputs = new CancoderIOInputsAutoLogged();
+        inputs.id = motor.getDeviceID();
+        position = motor.getPosition();
+        velocity = motor.getVelocity();
+    }
+
+    public void updateInputs() {
+        var status = BaseStatusSignal.refreshAll(
+            position,
+            velocity);
+        inputs.connected = connectedDebouncer.calculate(status.isOK());
+        inputs.positionRotations = position.getValueAsDouble();
+        inputs.velocityRPS = velocity.getValueAsDouble();
+    }
+
+    public void process() {
+        Logger.processInputs(name, inputs);
+    }
+}
diff --git a/src/main/java/frc/robot/lib/io/TalonFXIO.java b/src/main/java/frc/robot/lib/io/TalonFXIO.java
new file mode 100644
index 0000000..99d5a67
--- /dev/null
+++ b/src/main/java/frc/robot/lib/io/TalonFXIO.java
@@ -0,0 +1,89 @@
+package frc.robot.lib.io;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.filter.Debouncer;
+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.units.measure.Temperature;
+import edu.wpi.first.units.measure.Voltage;
+
+public class TalonFXIO {
+    @AutoLog
+    public static class TalonFXIOInputs {
+        public int id = 0;
+        public boolean connected = false;
+        public double positionRotations = 0.0;
+        public double velocityRPS = 0.0;
+        public double accelerationRPSS = 0.0;
+        public double appliedVolts = 0.0;
+        public double supplyVolts = 0.0;
+        public double statorCurrentAmps = 0.0;
+        public double supplyCurrentAmps = 0.0;
+        public double temperatureCelsius = 0.0;
+        public String controlRequest = "";
+    }
+
+    public final String name;
+    public final TalonFX motor;
+    public final TalonFXIOInputsAutoLogged inputs;
+
+    public StatusSignal<Angle> position;
+    public StatusSignal<AngularVelocity> velocity;
+    public StatusSignal<AngularAcceleration> acceleration;
+    public StatusSignal<Voltage> appliedVoltage;
+    public StatusSignal<Voltage> supplyVoltage;
+    public StatusSignal<Current> statorCurrent;
+    public StatusSignal<Current> supplyCurrent;
+    public StatusSignal<Temperature> temperature;
+            
+    private final Debouncer connectedDebouncer =
+        new Debouncer(0.5, Debouncer.DebounceType.kFalling);
+
+    public TalonFXIO(String name, TalonFX motor) {
+        this.name = name;
+        this.motor = motor;
+        inputs = new TalonFXIOInputsAutoLogged();
+        inputs.id = motor.getDeviceID();
+        position = motor.getPosition();
+        velocity = motor.getVelocity();
+        acceleration = motor.getAcceleration();
+        appliedVoltage = motor.getMotorVoltage();
+        supplyVoltage = motor.getSupplyVoltage();
+        statorCurrent = motor.getStatorCurrent();
+        supplyCurrent = motor.getSupplyCurrent();
+        temperature = motor.getDeviceTemp();
+    }
+
+    public void updateInputs() {
+        var status = BaseStatusSignal.refreshAll(
+            position,
+            velocity,
+            appliedVoltage,
+            supplyVoltage,
+            statorCurrent,
+            supplyCurrent,
+            temperature);
+        inputs.connected = connectedDebouncer.calculate(status.isOK());
+        inputs.positionRotations = position.getValueAsDouble();
+        inputs.velocityRPS = velocity.getValueAsDouble();
+        inputs.accelerationRPSS = acceleration.getValueAsDouble();
+        inputs.appliedVolts = appliedVoltage.getValueAsDouble();
+        inputs.supplyVolts = supplyVoltage.getValueAsDouble();
+        inputs.statorCurrentAmps = statorCurrent.getValueAsDouble();
+        inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble();
+        inputs.temperatureCelsius = temperature.getValueAsDouble();
+        inputs.controlRequest = motor.getAppliedControl().getName();
+    }
+
+    public void process() {
+        Logger.processInputs(name, inputs);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/lib/sim/FuelSim.java b/src/main/java/frc/robot/lib/sim/FuelSim.java
new file mode 100644
index 0000000..28ce7bc
--- /dev/null
+++ b/src/main/java/frc/robot/lib/sim/FuelSim.java
@@ -0,0 +1,861 @@
+// https://github.com/hammerheads5000/FuelSim
+package frc.robot.lib.sim;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Radians;
+
+import edu.wpi.first.math.geometry.Pose2d;
+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.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructArrayPublisher;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
+import java.util.ArrayList;
+import java.util.function.BooleanSupplier;
+import java.util.function.Supplier;
+
+public class FuelSim {
+    protected static final double PERIOD = 0.02; // sec
+    protected static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2
+    // Room temperature dry air density: https://en.wikipedia.org/wiki/Density_of_air#Dry_air
+    protected static final double AIR_DENSITY = 1.2041; // kg/m^3
+    protected static final double FIELD_COR = Math.sqrt(22 / 51.5); // coefficient of restitution with the field
+    protected static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel
+    protected static final double NET_COR = 0.2; // coefficient of restitution with the net
+    protected static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot
+    protected static final double FUEL_RADIUS = 0.075;
+    protected static final double FIELD_LENGTH = 16.51;
+    protected static final double FIELD_WIDTH = 8.04;
+    protected static final double TRENCH_WIDTH = 1.265;
+    protected static final double TRENCH_BLOCK_WIDTH = 0.305;
+    protected static final double TRENCH_HEIGHT = 0.565;
+    protected static final double TRENCH_BAR_HEIGHT = 0.102;
+    protected static final double TRENCH_BAR_WIDTH = 0.152;
+    protected static final double FRICTION = 0.1; // proportion of horizontal vel to lose per sec while on ground
+    protected static final double FUEL_MASS = 0.448 * 0.45392; // kgs
+    protected static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS;
+    // Drag coefficient of smooth sphere: https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg
+    protected static final double DRAG_COF = 0.47; // dimensionless
+    protected static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA;
+
+    protected static final Translation3d[] FIELD_XZ_LINE_STARTS = {
+        new Translation3d(0, 0, 0),
+        new Translation3d(3.96, 1.57, 0),
+        new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0),
+        new Translation3d(4.61, 1.57, 0.165),
+        new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165),
+        new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0),
+        new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0),
+        new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165),
+        new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165),
+        new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT),
+        new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT),
+        new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT),
+        new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT),
+        new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT),
+        new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT),
+        new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT),
+        new Translation3d(
+                FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT),
+    };
+
+    protected static final Translation3d[] FIELD_XZ_LINE_ENDS = {
+        new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0),
+        new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165),
+        new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165),
+        new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0),
+        new Translation3d(5.18, FIELD_WIDTH - 1.57, 0),
+        new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165),
+        new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165),
+        new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0),
+        new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0),
+        new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT),
+        new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT),
+        new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT),
+        new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT),
+        new Translation3d(
+                4.61 + TRENCH_BAR_WIDTH / 2, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT),
+        new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT),
+        new Translation3d(
+                FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2,
+                TRENCH_WIDTH + TRENCH_BLOCK_WIDTH,
+                TRENCH_HEIGHT + TRENCH_BAR_HEIGHT),
+        new Translation3d(FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT),
+    };
+
+    protected static class Fuel {
+        protected Translation3d pos;
+        protected Translation3d vel;
+
+        protected Fuel(Translation3d pos, Translation3d vel) {
+            this.pos = pos;
+            this.vel = vel;
+        }
+
+        protected Fuel(Translation3d pos) {
+            this(pos, new Translation3d());
+        }
+
+        protected void update(boolean simulateAirResistance, int subticks) {
+            pos = pos.plus(vel.times(PERIOD / subticks));
+            if (pos.getZ() > FUEL_RADIUS) {
+                Translation3d Fg = GRAVITY.times(FUEL_MASS);
+                Translation3d Fd = new Translation3d();
+
+                if (simulateAirResistance) {
+                    double speed = vel.getNorm();
+                    if (speed > 1e-6) {
+                        Fd = vel.times(-DRAG_FORCE_FACTOR * speed);
+                    }
+                }
+
+                Translation3d accel = Fg.plus(Fd).div(FUEL_MASS);
+                vel = vel.plus(accel.times(PERIOD / subticks));
+            }
+            if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) {
+                vel = new Translation3d(vel.getX(), vel.getY(), 0);
+                vel = vel.times(1 - FRICTION * PERIOD / subticks);
+                // pos = new Translation3d(pos.getX(), pos.getY(), FUEL_RADIUS);
+            }
+            handleFieldCollisions(subticks);
+        }
+
+        protected void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) {
+            if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) return; // not within y range
+            // Convert into 2D
+            Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ());
+            Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ());
+            Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ());
+            Translation2d lineVec = end2d.minus(start2d);
+
+            // Get closest point on line
+            Translation2d projected =
+                    start2d.plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm()));
+
+            if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm())
+                return; // projected point not on line
+            double dist = pos2d.getDistance(projected);
+            if (dist > FUEL_RADIUS) return; // not intersecting line
+            // Back into 3D
+            Translation3d normal = new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm());
+
+            // Apply collision response
+            pos = pos.plus(normal.times(FUEL_RADIUS - dist));
+            if (vel.dot(normal) > 0) return; // already moving away from line
+            vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal)));
+        }
+
+        protected void handleFieldCollisions(int subticks) {
+            // floor and bumps
+            for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) {
+                handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]);
+            }
+
+            // edges
+            if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) {
+                pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0));
+                vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0));
+            } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) {
+                pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0));
+                vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0));
+            }
+
+            if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) {
+                pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0));
+                vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0));
+            } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) {
+                pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0));
+                vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0));
+            }
+
+            // hubs
+            handleHubCollisions(Hub.BLUE_HUB, subticks);
+            handleHubCollisions(Hub.RED_HUB, subticks);
+
+            handleTrenchCollisions();
+        }
+
+        protected void handleHubCollisions(Hub hub, int subticks) {
+            hub.handleHubInteraction(this, subticks);
+            hub.fuelCollideSide(this);
+
+            double netCollision = hub.fuelHitNet(this);
+            if (netCollision != 0) {
+                pos = pos.plus(new Translation3d(netCollision, 0, 0));
+                vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ());
+            }
+        }
+
+        protected void handleTrenchCollisions() {
+            fuelCollideRectangle(
+                    this,
+                    new Translation3d(3.96, TRENCH_WIDTH, 0),
+                    new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT));
+            fuelCollideRectangle(
+                    this,
+                    new Translation3d(3.96, FIELD_WIDTH - 1.57, 0),
+                    new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT));
+            fuelCollideRectangle(
+                    this,
+                    new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0),
+                    new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT));
+            fuelCollideRectangle(
+                    this,
+                    new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0),
+                    new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT));
+            fuelCollideRectangle(
+                    this,
+                    new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT),
+                    new Translation3d(
+                            4.61 + TRENCH_BAR_WIDTH / 2,
+                            TRENCH_WIDTH + TRENCH_BLOCK_WIDTH,
+                            TRENCH_HEIGHT + TRENCH_BAR_HEIGHT));
+            fuelCollideRectangle(
+                    this,
+                    new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT),
+                    new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT));
+            fuelCollideRectangle(
+                    this,
+                    new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT),
+                    new Translation3d(
+                            FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2,
+                            TRENCH_WIDTH + TRENCH_BLOCK_WIDTH,
+                            TRENCH_HEIGHT + TRENCH_BAR_HEIGHT));
+            fuelCollideRectangle(
+                    this,
+                    new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT),
+                    new Translation3d(
+                            FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2,
+                            FIELD_WIDTH,
+                            TRENCH_HEIGHT + TRENCH_BAR_HEIGHT));
+        }
+
+        protected void addImpulse(Translation3d impulse) {
+            vel = vel.plus(impulse);
+        }
+    }
+
+    protected static void handleFuelCollision(Fuel a, Fuel b) {
+        Translation3d normal = a.pos.minus(b.pos);
+        double distance = normal.getNorm();
+        if (distance == 0) {
+            normal = new Translation3d(1, 0, 0);
+            distance = 1;
+        }
+        normal = normal.div(distance);
+        double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal));
+        double intersection = FUEL_RADIUS * 2 - distance;
+        a.pos = a.pos.plus(normal.times(intersection / 2));
+        b.pos = b.pos.minus(normal.times(intersection / 2));
+        a.addImpulse(normal.times(impulse));
+        b.addImpulse(normal.times(-impulse));
+    }
+
+    protected static final double CELL_SIZE = 0.25;
+    protected static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE);
+    protected static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE);
+
+    @SuppressWarnings("unchecked")
+    protected final ArrayList<Fuel>[][] grid = new ArrayList[GRID_COLS][GRID_ROWS];
+    private final ArrayList<ArrayList<Fuel>> activeCells = new ArrayList<>();
+
+    protected void handleFuelCollisions(ArrayList<Fuel> fuels) {
+        // Clear grid
+        for (ArrayList<Fuel> cell : activeCells) {
+            cell.clear();
+        }
+        activeCells.clear();
+
+        // Populate grid
+        for (Fuel fuel : fuels) {
+            int col = (int) (fuel.pos.getX() / CELL_SIZE);
+            int row = (int) (fuel.pos.getY() / CELL_SIZE);
+
+            if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) {
+                grid[col][row].add(fuel);
+                if (grid[col][row].size() == 1) {
+                   activeCells.add(grid[col][row]);
+                }
+            }
+        }
+
+        // Check collisions
+        for (Fuel fuel : fuels) {
+            int col = (int) (fuel.pos.getX() / CELL_SIZE);
+            int row = (int) (fuel.pos.getY() / CELL_SIZE);
+
+            // Check 3x3 neighbor cells
+            for (int i = col - 1; i <= col + 1; i++) {
+                for (int j = row - 1; j <= row + 1; j++) {
+                    if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) {
+                        for (Fuel other : grid[i][j]) {
+                            if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) {
+                                if (fuel.hashCode() < other.hashCode()) {
+                                    handleFuelCollision(fuel, other);
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    protected ArrayList<Fuel> fuels = new ArrayList<>();
+    protected boolean running = false;
+    protected boolean simulateAirResistance = false;
+    protected Supplier<Pose2d> robotPoseSupplier = null;
+    protected Supplier<ChassisSpeeds> robotFieldSpeedsSupplier = null;
+    protected double robotWidth; // size along the robot's y axis
+    protected double robotLength; // size along the robot's x axis
+    protected double bumperHeight;
+    protected ArrayList<SimIntake> intakes = new ArrayList<>();
+    protected int subticks = 5;
+
+    /**
+     * Creates a new instance of FuelSim
+     * @param tableKey NetworkTable to log fuel positions to as an array of {@link Translation3d} structs.
+     */
+    public FuelSim(String tableKey) {
+        // Initialize grid
+        for (int i = 0; i < GRID_COLS; i++) {
+            for (int j = 0; j < GRID_ROWS; j++) {
+                grid[i][j] = new ArrayList<Fuel>();
+            }
+        }
+
+        fuelPublisher = NetworkTableInstance.getDefault()
+                .getStructArrayTopic(tableKey + "/Fuels", Translation3d.struct)
+                .publish();
+    }
+
+    /**
+     * Creates a new instance of FuelSim with log path "/Fuel Simulation"
+     */
+    public FuelSim() {
+        this("/Fuel Simulation");
+    }
+
+    /**
+     * Clears the field of fuel
+     */
+    public void clearFuel() {
+        fuels.clear();
+    }
+
+    /**
+     * Spawns fuel in the neutral zone and depots
+     */
+    public void spawnStartingFuel() {
+        // Center fuel
+        Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS);
+        for (int i = 0; i < 15; i++) {
+            for (int j = 0; j < 6; j++) {
+                fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0))));
+                fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0))));
+                fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0))));
+                fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0))));
+            }
+        }
+
+        // Depots
+        for (int i = 0; i < 3; i++) {
+            for (int j = 0; j < 4; j++) {
+                fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS)));
+                fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS)));
+                fuels.add(new Fuel(
+                        new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS)));
+                fuels.add(new Fuel(
+                        new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS)));
+            }
+        }
+
+        // DEBUG: Log XZ lines
+        // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2];
+        // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) {
+        //     lines[i][0] = FIELD_XZ_LINE_STARTS[i];
+        //     lines[i][1] = FIELD_XZ_LINE_ENDS[i];
+        // }
+
+        // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines);
+    }
+
+    protected StructArrayPublisher<Translation3d> fuelPublisher;
+
+    /**
+     * Adds array of `Translation3d`'s to NetworkTables at tableKey + "/Fuels"
+     */
+    public void logFuels() {
+        fuelPublisher.set(fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new));
+    }
+
+    /**
+     * Start the simulation. `updateSim` must still be called every loop
+     */
+    public void start() {
+        running = true;
+    }
+
+    /**
+     * Pause the simulation.
+     */
+    public void stop() {
+        running = false;
+    }
+
+    /** Enables accounting for drag force in physics step **/
+    public void enableAirResistance() {
+        simulateAirResistance = true;
+    }
+
+    /**
+     * Sets the number of physics iterations per loop (0.02s)
+     * @param subticks
+     */
+    public void setSubticks(int subticks) {
+        this.subticks = subticks;
+    }
+
+    /**
+     * Registers a robot with the fuel simulator
+     * @param width from left to right (y-axis)
+     * @param length from front to back (x-axis)
+     * @param bumperHeight
+     * @param poseSupplier
+     * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier
+     */
+    public void registerRobot(
+            double width,
+            double length,
+            double bumperHeight,
+            Supplier<Pose2d> poseSupplier,
+            Supplier<ChassisSpeeds> fieldSpeedsSupplier) {
+        this.robotPoseSupplier = poseSupplier;
+        this.robotFieldSpeedsSupplier = fieldSpeedsSupplier;
+        this.robotWidth = width;
+        this.robotLength = length;
+        this.bumperHeight = bumperHeight;
+    }
+
+    /**
+     * Registers a robot with the fuel simulator
+     * @param width from left to right (y-axis)
+     * @param length from front to back (x-axis)
+     * @param bumperHeight from the ground
+     * @param poseSupplier
+     * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier
+     */
+    public void registerRobot(
+            Distance width,
+            Distance length,
+            Distance bumperHeight,
+            Supplier<Pose2d> poseSupplier,
+            Supplier<ChassisSpeeds> fieldSpeedsSupplier) {
+        this.robotPoseSupplier = poseSupplier;
+        this.robotFieldSpeedsSupplier = fieldSpeedsSupplier;
+        this.robotWidth = width.in(Meters);
+        this.robotLength = length.in(Meters);
+        this.bumperHeight = bumperHeight.in(Meters);
+    }
+
+    /**
+     * To be called periodically
+     * Will do nothing if sim is not running
+     */
+    public void updateSim() {
+        if (!running) return;
+
+        stepSim();
+    }
+
+    /**
+     * Run the simulation forward 1 time step (0.02s)
+     */
+    public void stepSim() {
+        for (int i = 0; i < subticks; i++) {
+            for (Fuel fuel : fuels) {
+                fuel.update(this.simulateAirResistance, this.subticks);
+            }
+
+            handleFuelCollisions(fuels);
+
+            if (robotPoseSupplier != null) {
+                handleRobotCollisions(fuels);
+                handleIntakes(fuels);
+            }
+        }
+
+        logFuels();
+    }
+
+    /**
+     * Adds a fuel onto the field
+     * @param pos Position to spawn at
+     * @param vel Initial velocity vector
+     */
+    public void spawnFuel(Translation3d pos, Translation3d vel) {
+        fuels.add(new Fuel(pos, vel));
+    }
+
+    /**
+     * Spawns a fuel onto the field with a specified launch velocity and angles, accounting for robot movement
+     * @param launchVelocity Initial launch velocity
+     * @param hoodAngle Hood angle where 0 is launching horizontally and 90 degrees is launching straight up
+     * @param turretYaw <i>Robot-relative</i> turret yaw
+     * @param launchHeight Height of the fuel to launch at. Make sure this is higher than your robot's bumper height, or else it will collide with your robot immediately.
+     * @throws IllegalStateException if robot is not registered
+     */
+    public void launchFuel(LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) {
+        if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) {
+            throw new IllegalStateException("Robot must be registered before launching fuel.");
+        }
+
+        Pose3d launchPose = new Pose3d(this.robotPoseSupplier.get())
+                .plus(new Transform3d(new Translation3d(Meters.zero(), Meters.zero(), launchHeight), Rotation3d.kZero));
+        ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get();
+
+        double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond);
+        double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond);
+        double xVel = horizontalVel
+                * Math.cos(
+                        turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians));
+        double yVel = horizontalVel
+                * Math.sin(
+                        turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians));
+
+        xVel += fieldSpeeds.vxMetersPerSecond;
+        yVel += fieldSpeeds.vyMetersPerSecond;
+
+        spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel));
+    }
+
+    protected void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) {
+        Translation2d relativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero)
+                .relativeTo(robot)
+                .getTranslation();
+
+        if (fuel.pos.getZ() > bumperHeight) return; // above bumpers
+        double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX();
+        double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX();
+        double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY();
+        double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY();
+
+        // not inside robot
+        if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) return;
+
+        Translation2d posOffset;
+        // find minimum distance to side and send corresponding collision response
+        if ((distanceToBottom >= distanceToTop
+                && distanceToBottom >= distanceToRight
+                && distanceToBottom >= distanceToLeft)) {
+            posOffset = new Translation2d(distanceToBottom, 0);
+        } else if ((distanceToTop >= distanceToBottom
+                && distanceToTop >= distanceToRight
+                && distanceToTop >= distanceToLeft)) {
+            posOffset = new Translation2d(-distanceToTop, 0);
+        } else if ((distanceToRight >= distanceToBottom
+                && distanceToRight >= distanceToTop
+                && distanceToRight >= distanceToLeft)) {
+            posOffset = new Translation2d(0, distanceToRight);
+        } else {
+            posOffset = new Translation2d(0, -distanceToLeft);
+        }
+
+        posOffset = posOffset.rotateBy(robot.getRotation());
+        fuel.pos = fuel.pos.plus(new Translation3d(posOffset));
+        Translation2d normal = posOffset.div(posOffset.getNorm());
+        if (fuel.vel.toTranslation2d().dot(normal) < 0)
+            fuel.addImpulse(
+                    new Translation3d(normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR))));
+        if (robotVel.dot(normal) > 0) fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal))));
+    }
+
+    protected void handleRobotCollisions(ArrayList<Fuel> fuels) {
+        Pose2d robot = robotPoseSupplier.get();
+        ChassisSpeeds speeds = robotFieldSpeedsSupplier.get();
+        Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+
+        for (Fuel fuel : fuels) {
+            handleRobotCollision(fuel, robot, robotVel);
+        }
+    }
+
+    protected void handleIntakes(ArrayList<Fuel> fuels) {
+        Pose2d robot = robotPoseSupplier.get();
+        for (SimIntake intake : intakes) {
+            for (int i = 0; i < fuels.size(); i++) {
+                if (intake.shouldIntake(fuels.get(i), robot)) {
+                    fuels.remove(i);
+                    i--;
+                }
+            }
+        }
+    }
+
+    protected static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) {
+        if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS)
+            return; // above rectangle
+        double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX();
+        double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS;
+        double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS;
+        double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY();
+
+        // not inside hub
+        if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) return;
+
+        Translation2d collision;
+        // find minimum distance to side and send corresponding collision response
+        if (fuel.pos.getX() < start.getX()
+                || (distanceToLeft >= distanceToRight
+                        && distanceToLeft >= distanceToTop
+                        && distanceToLeft >= distanceToBottom)) {
+            collision = new Translation2d(distanceToLeft, 0);
+        } else if (fuel.pos.getX() >= end.getX()
+                || (distanceToRight >= distanceToLeft
+                        && distanceToRight >= distanceToTop
+                        && distanceToRight >= distanceToBottom)) {
+            collision = new Translation2d(-distanceToRight, 0);
+        } else if (fuel.pos.getY() > end.getY()
+                || (distanceToTop >= distanceToLeft
+                        && distanceToTop >= distanceToRight
+                        && distanceToTop >= distanceToBottom)) {
+            collision = new Translation2d(0, -distanceToTop);
+        } else {
+            collision = new Translation2d(0, distanceToBottom);
+        }
+
+        if (collision.getX() != 0) {
+            fuel.pos = fuel.pos.plus(new Translation3d(collision));
+            fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0));
+        } else if (collision.getY() != 0) {
+            fuel.pos = fuel.pos.plus(new Translation3d(collision));
+            fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0));
+        }
+    }
+
+    /**
+     * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter.
+     * @param xMin Minimum x position for the bounding box
+     * @param xMax Maximum x position for the bounding box
+     * @param yMin Minimum y position for the bounding box
+     * @param yMax Maximum y position for the bounding box
+     * @param ableToIntake Should a return a boolean whether the intake is active
+     * @param intakeCallback Function to call when a fuel is intaked
+     */
+    public void registerIntake(
+            double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake, Runnable intakeCallback) {
+        intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback));
+    }
+
+    /**
+     * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter.
+     * @param xMin Minimum x position for the bounding box
+     * @param xMax Maximum x position for the bounding box
+     * @param yMin Minimum y position for the bounding box
+     * @param yMax Maximum y position for the bounding box
+     * @param ableToIntake Should a return a boolean whether the intake is active
+     */
+    public void registerIntake(double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) {
+        registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> {});
+    }
+
+    /**
+     * Registers an intake with the fuel simulator. This intake will always remove fuel from the field.
+     * @param xMin Minimum x position for the bounding box
+     * @param xMax Maximum x position for the bounding box
+     * @param yMin Minimum y position for the bounding box
+     * @param yMax Maximum y position for the bounding box
+     * @param intakeCallback Function to call when a fuel is intaked
+     */
+    public void registerIntake(double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) {
+        registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback);
+    }
+
+    /**
+     * Registers an intake with the fuel simulator. This intake will always remove fuel from the field.
+     * @param xMin Minimum x position for the bounding box
+     * @param xMax Maximum x position for the bounding box
+     * @param yMin Minimum y position for the bounding box
+     * @param yMax Maximum y position for the bounding box
+     */
+    public void registerIntake(double xMin, double xMax, double yMin, double yMax) {
+        registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> {});
+    }
+
+    /**
+     * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter.
+     * @param xMin Minimum x position for the bounding box
+     * @param xMax Maximum x position for the bounding box
+     * @param yMin Minimum y position for the bounding box
+     * @param yMax Maximum y position for the bounding box
+     * @param ableToIntake Should a return a boolean whether the intake is active
+     * @param intakeCallback Function to call when a fuel is intaked
+     */
+    public void registerIntake(
+            Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake, Runnable intakeCallback) {
+        registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake, intakeCallback);
+    }
+
+    /**
+     * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter.
+     * @param xMin Minimum x position for the bounding box
+     * @param xMax Maximum x position for the bounding box
+     * @param yMin Minimum y position for the bounding box
+     * @param yMax Maximum y position for the bounding box
+     * @param ableToIntake Should a return a boolean whether the intake is active
+     */
+    public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake) {
+        registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake);
+    }
+
+    /**
+     * Registers an intake with the fuel simulator. This intake will always remove fuel from the field.
+     * @param xMin Minimum x position for the bounding box
+     * @param xMax Maximum x position for the bounding box
+     * @param yMin Minimum y position for the bounding box
+     * @param yMax Maximum y position for the bounding box
+     * @param intakeCallback Function to call when a fuel is intaked
+     */
+    public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax, Runnable intakeCallback) {
+        registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), intakeCallback);
+    }
+
+    /**
+     * Registers an intake with the fuel simulator. This intake will always remove fuel from the field.
+     * @param xMin Minimum x position for the bounding box
+     * @param xMax Maximum x position for the bounding box
+     * @param yMin Minimum y position for the bounding box
+     * @param yMax Maximum y position for the bounding box
+     */
+    public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax) {
+        registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters));
+    }
+
+    public static class Hub {
+        public static final Hub BLUE_HUB =
+                new Hub(new Translation2d(4.61, FIELD_WIDTH / 2), new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), 1);
+        public static final Hub RED_HUB = new Hub(
+                new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2),
+                new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89),
+                -1);
+
+        protected static final double ENTRY_HEIGHT = 1.83;
+        protected static final double ENTRY_RADIUS = 0.56;
+
+        protected static final double SIDE = 1.2;
+
+        protected static final double NET_HEIGHT_MAX = 3.057;
+        protected static final double NET_HEIGHT_MIN = 1.5;
+        protected static final double NET_OFFSET = SIDE / 2 + 0.261;
+        protected static final double NET_WIDTH = 1.484;
+
+        protected final Translation2d center;
+        protected final Translation3d exit;
+        protected final int exitVelXMult;
+
+        protected int score = 0;
+
+        protected Hub(Translation2d center, Translation3d exit, int exitVelXMult) {
+            this.center = center;
+            this.exit = exit;
+            this.exitVelXMult = exitVelXMult;
+        }
+
+        protected void handleHubInteraction(Fuel fuel, int subticks) {
+            if (didFuelScore(fuel, subticks)) {
+                fuel.pos = exit;
+                fuel.vel = getDispersalVelocity();
+                score++;
+            }
+        }
+
+        protected boolean didFuelScore(Fuel fuel, int subticks) {
+            return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS
+                    && fuel.pos.getZ() <= ENTRY_HEIGHT
+                    && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT;
+        }
+
+        protected Translation3d getDispersalVelocity() {
+            return new Translation3d(exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0);
+        }
+
+        /**
+         * Reset this hub's score to 0
+         */
+        public void resetScore() {
+            score = 0;
+        }
+
+        /**
+         * Get the current count of fuel scored in this hub
+         * @return
+         */
+        public int getScore() {
+            return score;
+        }
+
+        protected void fuelCollideSide(Fuel fuel) {
+            fuelCollideRectangle(
+                    fuel,
+                    new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0),
+                    new Translation3d(center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1));
+        }
+
+        protected double fuelHitNet(Fuel fuel) {
+            if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) return 0;
+            if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 || fuel.pos.getY() < center.getY() - NET_WIDTH / 2)
+                return 0;
+            if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) {
+                return Math.max(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS));
+            } else {
+                return Math.min(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS));
+            }
+        }
+    }
+
+    protected class SimIntake {
+        double xMin, xMax, yMin, yMax;
+        BooleanSupplier ableToIntake;
+        Runnable callback;
+
+        protected SimIntake(
+                double xMin,
+                double xMax,
+                double yMin,
+                double yMax,
+                BooleanSupplier ableToIntake,
+                Runnable intakeCallback) {
+            this.xMin = xMin;
+            this.xMax = xMax;
+            this.yMin = yMin;
+            this.yMax = yMax;
+            this.ableToIntake = ableToIntake;
+            this.callback = intakeCallback;
+        }
+
+        protected boolean shouldIntake(Fuel fuel, Pose2d robotPose) {
+            if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) return false;
+
+            Translation2d fuelRelativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero)
+                    .relativeTo(robotPose)
+                    .getTranslation();
+
+            boolean result = fuelRelativePos.getX() >= xMin
+                    && fuelRelativePos.getX() <= xMax
+                    && fuelRelativePos.getY() >= yMin
+                    && fuelRelativePos.getY() <= yMax;
+            if (result) {
+                callback.run();
+            }
+            return result;
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/TelemetryManager.java b/src/main/java/frc/robot/subsystems/TelemetryManager.java
index 6a1047d..6bfe4d7 100644
--- a/src/main/java/frc/robot/subsystems/TelemetryManager.java
+++ b/src/main/java/frc/robot/subsystems/TelemetryManager.java
@@ -119,6 +119,11 @@ public static void makeSendableTalonFX(String name, TalonFX motor, SendableBuild
                 .getValue()
                 .in(Units.Celsius),
             null);
+        builder.addStringProperty(name + "/Request", 
+            () -> motor
+                .getAppliedControl()
+                .getName(),
+            null);
     }
 
     public void addSendable(Sendable sendable) {
diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java
new file mode 100644
index 0000000..faa6504
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/climb/Climb.java
@@ -0,0 +1,168 @@
+package frc.robot.subsystems.climb;
+
+import static edu.wpi.first.units.Units.*;
+import static frc.robot.subsystems.climb.ClimbConstants.*;
+
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.subsystems.climb.ClimbConstants.Setpoint;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+
+public class Climb extends SubsystemBase {
+	private static Climb climbInstance;
+	public static Climb getInstance() {
+		if (climbInstance == null) {
+			climbInstance = new Climb();
+		}
+		return climbInstance;
+	}
+
+	private final TalonFX climbMotor;
+
+	private double lastReadHeight;
+	private double lastReadSpeed;
+
+	private double targetHeight = END_EFFECTOR_HEIGHT;
+	private ControlRequest request = new NeutralOut();
+
+	private ElevatorSim sim;
+
+	private ClimbIO io;
+
+	private Climb() {
+		super();
+		climbMotor = new TalonFX(CLIMB_MOTOR_ID,"CV");
+		climbMotor.getConfigurator().apply(getConfig());
+		climbMotor.setNeutralMode(NeutralModeValue.Brake);
+		if (Robot.isSimulation()) {
+			sim = new ElevatorSim(
+				DCMotor.getKrakenX60(1),
+				GEAR_RATIO,
+				1.13,
+				0.05,
+				0.0,
+				1,
+				true,
+				0.0,
+				0.0, 0.0
+			);
+		}
+		io = new ClimbIO(getName(), climbMotor);
+	}
+
+	@Override
+	public void periodic() {
+		// Read the height from the motor encoder
+		lastReadHeight = 
+			climbMotor.getPosition().getValueAsDouble();
+		lastReadSpeed = 
+			climbMotor.getVelocity().getValueAsDouble();
+		// updates the motor
+		climbMotor.setControl(request);
+
+		io.updateInputs(lastReadHeight, lastReadSpeed, getCurrentCommand(), getDefaultCommand());
+		io.process();
+	}
+
+	@Override
+	public void simulationPeriodic() {
+		climbMotor.getSimState().setSupplyVoltage(12);
+		sim.setInput(climbMotor.getSimState().getMotorVoltage());
+		sim.update(0.020);
+		climbMotor.getSimState().setRawRotorPosition(sim.getPositionMeters() * CONVERSION_FACTOR * 1 / Constants.TAU);
+		climbMotor.getSimState().setRotorVelocity(
+			sim.getVelocityMetersPerSecond() * CONVERSION_FACTOR * 1 / Constants.TAU);
+	}
+
+	/** Swaps the control request */
+	private void setRequest(ControlRequest request) {
+		this.request = request;
+	}
+
+	public Command moveToScoringHeight(Setpoint height) {
+		return moveToTarget(height.height).withName(height.name() + ": Move To Height");
+	}
+
+	/** Attempts to move the end effector to a height, in meters */
+	private Command moveToTarget(double targetHeight) {
+		return runOnce(
+			() -> {
+				this.targetHeight = targetHeight;
+				setRequest(new MotionMagicVoltage(
+						targetHeight));
+			}
+		).andThen(
+			Commands.waitUntil(() -> isNearTarget())
+		).withName(String.format("%.2f: Unknown, Moving", targetHeight));
+	}
+
+	/** Stops the elevator */
+	public Command stop() {
+		return runOnce(
+			() -> setRequest(
+				new NeutralOut()))
+			.withName("Stopped");
+	}
+
+	/** Sysid commands
+	 * @param dynamic If true, then runs dynamic test. If false, quasistatic
+	 */	
+	public Command sysId(boolean dynamic, SysIdRoutine.Direction direction) {
+		return defer(() -> {
+			VoltageOut request = new VoltageOut(0);
+			SysIdRoutine sysIdRoutine = new SysIdRoutine(
+				new SysIdRoutine.Config(
+					null, // Default ramp rate (1 V)
+					null, // Default step voltage (7 V)
+					null // Use default timeout (10 s)
+				),
+				new SysIdRoutine.Mechanism(
+					output -> request.withOutput(output),					
+                	log -> {
+						log.motor("climbMotor")
+							.voltage(Volts.of(request.Output))
+							.linearPosition(
+								Meters.of(
+									climbMotor.getPosition().getValueAsDouble()))
+							.linearVelocity(
+								MetersPerSecond.of(
+									climbMotor.getVelocity().getValueAsDouble()));
+					},
+					this
+				)
+			);
+			if (dynamic) {
+				return sysIdRoutine.dynamic(direction);
+			} else {
+				return sysIdRoutine.quasistatic(direction);
+			}
+		});
+	}
+
+	/** Checks if the end effector is within 1 cm of the target */
+	public boolean isNearTarget() {
+		return MathUtil.isNear(
+			lastReadHeight,
+			targetHeight,
+			EPSILON);
+	}
+	// command
+	
+	public Command hangCommand() {
+		return moveToScoringHeight(Setpoint.UP)
+			.andThen(Climb.getInstance().moveToScoringHeight(Setpoint.BASE));
+	}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java
new file mode 100644
index 0000000..530b3ab
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java
@@ -0,0 +1,64 @@
+package frc.robot.subsystems.climb;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
+
+public class ClimbConstants {
+
+    public static final double EPSILON = 0.03; // Meters
+
+    public static final double SPROCKET_RADIUS = 0.0412; // Effective pitch radius
+    public static final double GEAR_RATIO = 9;
+    public static final double CONVERSION_FACTOR = 67;
+
+    public static final double SPROCKET_CIRCUMFERENCE = SPROCKET_RADIUS * Constants.TAU;
+    public static final double END_EFFECTOR_HEIGHT = 0.0; // Meters // CHANGE THIS
+    public static final double METERS_PER_ROTATION = 0.028776; // Approximated using measurement
+    public static final double CARRIAGE_WEIGHT = 7.55; // kg
+
+    public static final double MAX_ACCEL = 1.5;
+    public static final double MAX_SPEED = 1.0; // m/s
+    
+    public static final int CLIMB_MOTOR_ID = 41; // change this
+
+    public static enum Setpoint {
+        BASE(0.003), // small offset to prevent stalling (allegedly)
+        UP(Units.inchesToMeters(5.0));
+        public final double height;
+        private Setpoint(double height) {
+            this.height = height;
+        }
+    }
+
+    public static TalonFXConfiguration getConfig() {
+        return new TalonFXConfiguration()
+            .withSlot0(new Slot0Configs()
+                .withKS(0.125)
+                .withKV(0.0)
+                .withKP(1.0)
+                .withKI(0.0)
+                .withKD(0.05)
+                .withKG(0.375))
+            .withMotionMagic(new MotionMagicConfigs()
+                .withMotionMagicAcceleration(MAX_ACCEL) // 1.0 m/s^2
+                .withMotionMagicCruiseVelocity(MAX_SPEED)
+                .withMotionMagicJerk(320))
+            .withCurrentLimits(new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(60)
+                .withSupplyCurrentLimit(40)
+                .withStatorCurrentLimitEnable(true)
+                .withSupplyCurrentLimitEnable(true))
+            .withVoltage(new VoltageConfigs()
+                .withPeakForwardVoltage(12.0)
+                .withPeakReverseVoltage(-12.0))
+            .withFeedback(new FeedbackConfigs()
+                .withSensorToMechanismRatio(CONVERSION_FACTOR));
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java
new file mode 100644
index 0000000..0daee10
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java
@@ -0,0 +1,44 @@
+
+package frc.robot.subsystems.climb;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.lib.io.TalonFXIO;
+
+public  class ClimbIO {
+    @AutoLog
+    public static class ClimbIOInputs {
+        public double positionMeters = 0.0;
+        public double velocityMetersPerSecond = 0.0;
+        public String currentCommand = "";
+        public String defaultCommand = "";
+    }
+
+    private final String name;
+    private final TalonFXIO motorIO;
+    private final ClimbIOInputsAutoLogged inputs;
+
+    public ClimbIO(String name, TalonFX motor) {
+        this.name = name;
+        motorIO = new TalonFXIO(name + "/Motor", motor);
+        inputs = new ClimbIOInputsAutoLogged();
+    }
+
+    public void updateInputs(double positionMeters, double velocityMetersPerSecond, Command currentCommand, Command defaultCommand) {
+        motorIO.updateInputs();
+
+        inputs.positionMeters = positionMeters;
+        inputs.velocityMetersPerSecond = velocityMetersPerSecond;
+        inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None";
+        inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None";
+    }
+
+    public void process() {
+        motorIO.process();
+        Logger.processInputs(name, inputs);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 9685396..bf3cb11 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -2,10 +2,10 @@
 
 import static frc.robot.subsystems.drive.DriveConstants.*;
 
-import java.util.function.Consumer;
-import java.util.function.Function;
 import java.util.function.Supplier;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
 import com.therekrab.autopilot.APTarget;
@@ -20,28 +20,30 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.units.BaseUnits;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Time;
-import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Robot;
+import frc.robot.lib.control.ControlConstants.PIDVConstants;
+import frc.robot.lib.control.ControlConstants.ProfiledPIDVConstants;
+import frc.robot.lib.control.ProfiledPIDVController;
 import frc.robot.lib.field.FieldLayout;
 import frc.robot.lib.trajectory.LocalADStarWrapper;
-import frc.robot.lib.util.TunableNumber;
 import frc.robot.lib.util.Util;
 import frc.robot.subsystems.TelemetryManager;
-import frc.robot.subsystems.drive.ctre.CtreDriveConstants;
+import frc.robot.subsystems.drive.ctre.CompCtreDriveConstants;
+// import frc.robot.subsystems.shooter.ShotCalculator;
 import frc.robot.subsystems.drive.commands.AutopilotCommand;
 import frc.robot.subsystems.drive.commands.PIDToPoseCommand;
 import frc.robot.subsystems.drive.commands.TrajectoryCommand;
 import frc.robot.subsystems.drive.ctre.CtreDrive;
-import frc.robot.subsystems.drive.ctre.CtreDriveTelemetry;
-import frc.robot.subsystems.vision.VisionConstants;
+// import frc.robot.subsystems.drive.ctre.CtreDriveTelemetry;
 
 public class Drive extends SubsystemBase {
 	private static Drive driveInstance;
@@ -53,24 +55,26 @@ public static Drive getInstance() {
 	}
 
 	private SwerveDriveState lastReadState;
-	public final SwerveRequest.FieldCentric teleopRequest;
-	public SwerveRequest driveRequest;
+	private SwerveDriveState prevReadState;
+	public static SwerveRequest.FieldCentric teleopRequest = new SwerveRequest.FieldCentric();
+	public SwerveRequest driveRequest = teleopRequest;
 
 	private final CtreDrive drivetrain;   
-	private final CtreDriveTelemetry telemetry;
+	// private final CtreDriveTelemetry telemetry;
 	@SuppressWarnings("unused") 
 	private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); // Citrus what are you doing
 	
 	private final LocalADStarWrapper pathfinder;
 
+	private final DriveIO io;
+
 	private Drive() {
-		drivetrain = CtreDriveConstants.createDrivetrain();  
-		drivetrain.setVisionMeasurementStdDevs(VisionConstants.LOCAL_MEASUREMENT_STD_DEVS);
-		drivetrain.setStateStdDevs(VisionConstants.STATE_STD_DEVS);
-		telemetry = new CtreDriveTelemetry(MAX_SPEED);  
+		drivetrain = CompCtreDriveConstants.createDrivetrain();  
+		// telemetry = new CtreDriveTelemetry(MAX_SPEED);  
 		teleopRequest = new SwerveRequest.FieldCentric();
 		driveRequest = teleopRequest;
 		lastReadState = drivetrain.getState();
+		prevReadState = lastReadState;
 		drivetrain.setDefaultCommand(drivetrain.applyRequest(() -> {
 			return driveRequest;
 		}));
@@ -79,30 +83,8 @@ private Drive() {
 
 		drivetrain.getOdometryThread().setThreadPriority(31);
 		TelemetryManager.getInstance().addStructPublisher("Mechanisms/Drive", Pose3d.struct, () -> new Pose3d(getPose()));
-		// TelemetryManager.getInstance().addStructPublisher("Drive/TargetSpeeds", ChassisSpeeds.struct,
-		// 	() -> {
-		// 		try {
-		// 			if (driveRequest instanceof SwerveRequest.ApplyFieldSpeeds) {
-		// 				return ChassisSpeeds.fromFieldRelativeSpeeds(
-		// 					((SwerveRequest.ApplyFieldSpeeds) driveRequest).Speeds, 
-		// 					lastReadState.Pose.getRotation());
-		// 			} else if (driveRequest instanceof SwerveRequest.ApplyRobotSpeeds) {
-		// 				return ((SwerveRequest.ApplyRobotSpeeds) driveRequest).Speeds;
-		// 			} else if (driveRequest instanceof SwerveRequest.FieldCentric) {
-		// 				var req = ((SwerveRequest.FieldCentric) driveRequest);
-		// 				return ChassisSpeeds.fromFieldRelativeSpeeds(
-		// 					req.VelocityX, 
-		// 					req.VelocityY, 
-		// 					req.RotationalRate,
-		// 					lastReadState.Pose.getRotation());
-		// 			} else if (driveRequest instanceof SwerveRequest.RobotCentric) {
-		// 				var req = ((SwerveRequest.RobotCentric) driveRequest);
-		// 				return new ChassisSpeeds(req.VelocityX, req.VelocityY, req.RotationalRate);
-		// 			}
-		// 		} finally {}
-		// 		return lastReadState.Speeds;
-		// 	});
-		TelemetryManager.getInstance().addSendable(this);
+		io = new DriveIO(getName(), drivetrain);
+		// TelemetryManager.getInstance().addSendable(this);
 	}
 
 	/** @return the ctre generated drivetrain */
@@ -112,13 +94,18 @@ public CtreDrive getCtreDrive() {
 
 	@Override
 	public void periodic() {
+		prevReadState = lastReadState;
 		lastReadState = drivetrain.getState();
 		outputTelemetry();
 	}
 
 	public void outputTelemetry() {
-		telemetry.telemeterize(lastReadState);
+		// telemetry.telemeterize(lastReadState);
 		FieldLayout.field.setRobotPose(getPose());
+		io.updateInputs(driveRequest, lastReadState, getCurrentCommand(), getDefaultCommand());
+		io.process();
+
+		// SmartDashboard.putNumber("toShooter", FieldLayout.APRILTAG_MAP.getTagPose(10).get().toPose2d().getTranslation().getDistance(getPose().getTranslation()));
 	}
 
 	/**
@@ -142,6 +129,10 @@ public ChassisSpeeds getFieldSpeeds() {
 		return ChassisSpeeds.fromRobotRelativeSpeeds(lastReadState.Speeds, lastReadState.Pose.getRotation());
 	}
 	
+	public ChassisSpeeds getPrevFieldSpeeds() {
+		return ChassisSpeeds.fromRobotRelativeSpeeds(prevReadState.Speeds, prevReadState.Pose.getRotation());
+	}
+	
 	/**
 	 * Switches the swerve request
 	 * <p>Please do not the new swerve request every 20 ms</p>
@@ -172,9 +163,9 @@ public Command openLoopControl() {
             double yFancy = xy[1];
             double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND);
 
-			SmartDashboard.putNumber("Sticks/vX", xDesiredRaw);
-			SmartDashboard.putNumber("Sticks/vY", yDesiredRaw);
-			SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw);
+			// SmartDashboard.putNumber("Sticks/vX", xDesiredRaw);
+			// SmartDashboard.putNumber("Sticks/vY", yDesiredRaw);
+			// SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw);
 
 			teleopRequest
 				.withVelocityX(xFancy * MAX_SPEED)
@@ -183,19 +174,49 @@ public Command openLoopControl() {
 		}).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop");
 	}
 
+	public Command nudgeCommand() {
+        return runOnce(() -> {
+            teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0);
+            setSwerveRequest(teleopRequest);
+        }).andThen(run(() -> {
+			int pov = Robot.controller.getHID().getPOV();		
+            double xDesiredRaw = Math.cos(pov * Math.PI / 180.0);
+            double yDesiredRaw = - Math.sin(pov * Math.PI / 180.0);
+
+            double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND);
+            double xFancy = xy[0];
+            double yFancy = xy[1];
+
+			teleopRequest
+				.withVelocityX(xFancy * 0.4)
+				.withVelocityY(yFancy * 0.4);        
+		}).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Nudge");
+	}
+
 	/** 
 	 * Locks the robot onto a pose. 
 	 * Utilizes feedforwards derived from the current chassis speeds
 	 */
-	public Command headingLockToPose(Pose2d pose) {
-		SwerveRequest.FieldCentricFacingAngle request = 
-			new SwerveRequest.FieldCentricFacingAngle()
-				.withHeadingPID(8, 0, 0.00)
-				.withMaxAbsRotationalRate(MAX_ROTATION_SPEED);
+	public Command headingLockToPose(Translation2d pose) {
+		SwerveRequest.FieldCentric request = 
+			new SwerveRequest.FieldCentric();
+		
+		ProfiledPIDVController thetaController = 
+			new ProfiledPIDVController(
+				new ProfiledPIDVConstants(
+					new PIDVConstants(10.0, 0.0, 1), 
+					new TrapezoidProfile.Constraints(Math.PI * 16, Math.PI * 5))
+			);
+		thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
 		return runOnce(() -> {
-			request.withVelocityX(0).withVelocityY(0).withTargetDirection(getPose().getRotation());
+			request.withVelocityX(0).withVelocityY(0)
+				.withRotationalRate(0);
 			setSwerveRequest(request);
+			
+			thetaController.setInitialSetpoint(
+				getPose().getRotation().getRadians(), 
+				getState().Speeds.omegaRadiansPerSecond);
 		}).andThen(
 			run(() -> {
 				double xDesiredRaw = -Robot.controller.getLeftY();
@@ -206,41 +227,58 @@ public Command headingLockToPose(Pose2d pose) {
 				double yFancy = xy[1];
 
 				var state = getState();
-				var delta = pose.getTranslation().minus(getPose().getTranslation());
+				var delta = pose.minus(getPose().getTranslation());
 				var targetDirection = delta.getAngle();
+				
+				
 				var normSq = delta.getNorm() * delta.getNorm();
 				var fieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(state.Speeds, getPose().getRotation());
 				var rotationalRate = normSq > 1e-4 ? 
 					(-delta.getX() * fieldSpeeds.vyMetersPerSecond
 					+ delta.getY() * fieldSpeeds.vxMetersPerSecond)
 					/ (normSq) : 0.0;
+				
+				var rotation = thetaController
+					.setTarget(targetDirection.getRadians(), rotationalRate)
+					.setMeasurement(state.Pose.getRotation().getRadians(), state.Speeds.omegaRadiansPerSecond)
+					.getOutput();
 
-				// SmartDashboard.putNumber("error tracking", 
-				// 	MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180
-				// ));
+				SmartDashboard.putNumber("error tracking", 
+					MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180
+				));
 
 				request
 					// .withHeadingPID(p.get(), i.get(), d.get())
 					.withVelocityX(xFancy * MAX_SPEED)
 					.withVelocityY(yFancy * MAX_SPEED)
-					.withTargetDirection(targetDirection)
-					.withTargetRateFeedforward(rotationalRate * 1.0);
+					.withRotationalRate(rotation);
 			}).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Heading Lock");
 	}
 
 	/** 
-	 * Locks the robot onto a pose, with TOF Adjustment
+	 * Locks the robot onto a pose. 
 	 * Utilizes feedforwards derived from the current chassis speeds
 	 */
-	public Command headingLockToPoseWithTOFAdjustment(Pose2d pose, Function<Double, Double> tof, Consumer<Double> tofAcceptor) {
-		SwerveRequest.FieldCentricFacingAngle request = 
-			new SwerveRequest.FieldCentricFacingAngle()
-				.withHeadingPID(25, 0, 0.01)
-				.withMaxAbsRotationalRate(MAX_ROTATION_SPEED);
+	public Command headingLockToPose(Supplier<Pose2d> pose) {
+		SwerveRequest.FieldCentric request = 
+			new SwerveRequest.FieldCentric();
+		
+		ProfiledPIDVController thetaController = 
+			new ProfiledPIDVController(
+				new ProfiledPIDVConstants(
+					new PIDVConstants(10.0, 0.0, 1), 
+					new TrapezoidProfile.Constraints(Math.PI * 16, Math.PI * 5))
+			);
+		thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
 		return runOnce(() -> {
-			request.withVelocityX(0).withVelocityY(0).withTargetDirection(getPose().getRotation());
+			request.withVelocityX(0).withVelocityY(0)
+				.withRotationalRate(0);
 			setSwerveRequest(request);
+			
+			thetaController.setInitialSetpoint(
+				getPose().getRotation().getRadians(), 
+				getState().Speeds.omegaRadiansPerSecond);
 		}).andThen(
 			run(() -> {
 				double xDesiredRaw = -Robot.controller.getLeftY();
@@ -251,29 +289,52 @@ public Command headingLockToPoseWithTOFAdjustment(Pose2d pose, Function<Double,
 				double yFancy = xy[1];
 
 				var state = getState();
-				var delta = pose.getTranslation().minus(getPose().getTranslation());
+				var delta = pose.get().getTranslation().minus(getPose().getTranslation());
 				var targetDirection = delta.getAngle();
-				var normSq = delta.getSquaredNorm();
+				
+				var normSq = delta.getNorm() * delta.getNorm();
 				var fieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(state.Speeds, getPose().getRotation());
 				var rotationalRate = normSq > 1e-4 ? 
 					(-delta.getX() * fieldSpeeds.vyMetersPerSecond
 					+ delta.getY() * fieldSpeeds.vxMetersPerSecond)
 					/ (normSq) : 0.0;
+				
+				var rotation = thetaController
+					.setTarget(targetDirection.getRadians(), rotationalRate)
+					.setMeasurement(state.Pose.getRotation().getRadians(), state.Speeds.omegaRadiansPerSecond)
+					.getOutput();
 
-				SmartDashboard.putNumber("error tracking", 
-					MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180
-				));
+				Logger.recordOutput("Tracking Error", 
+					MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180));
 
-				Translation2d speedVector = new Translation2d(fieldSpeeds.vxMetersPerSecond, fieldSpeeds.vyMetersPerSecond);
-				
 				request
+					// .withHeadingPID(p.get(), i.get(), d.get())
 					.withVelocityX(xFancy * MAX_SPEED)
 					.withVelocityY(yFancy * MAX_SPEED)
-					.withTargetDirection(targetDirection)
-					.withTargetRateFeedforward(rotationalRate * 1.5);
+					.withRotationalRate(rotation);
 			}).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Heading Lock");
 	}
 
+	/** 
+	 * Locks the robot onto a pose, with TOF Adjustment
+	 * Utilizes feedforwards derived from the current chassis speeds
+	 */
+	public Command headingLockToHub() {
+		// TelemetryManager.getInstance().addStructPublisher(
+		// 	"thing", Pose2d.struct, () -> new Pose2d(Constants.FieldConstants.allianceCorrected(
+		// 			FieldPoses.HUB.pose3d.getTranslation()
+		// 			// Constants.FieldConstants.Hub.topCenterPoint
+		// 			).toTranslation2d(), Rotation2d.kZero));
+
+		return headingLockToPose(
+				Constants.FieldConstants
+					.allianceCorrected(
+						FieldPoses.HUB.pose3d.getTranslation()
+						// Constants.FieldConstants.Hub.topCenterPoint
+						)
+							.toTranslation2d());
+	}
+
 	/** 
 	 * Auto aligns to the nearest reef face
 	 * @param left chooses the left or right face
@@ -365,46 +426,46 @@ && isRollStable()
 			&& Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(MAX_ROTATION_SPEED_SCORING);
 	}
 
-	@Override
-	public void initSendable(SendableBuilder builder) {
-		super.initSendable(builder);
-		builder.addDoubleProperty(
-			"Pitch Velocity Degrees Per Second",
-			() -> drivetrain
-				.getPigeon2()
-				.getAngularVelocityYDevice()
-				.getValue()
-				.in(Units.DegreesPerSecond),
-			null);
-		builder.addDoubleProperty(
-			"Pitch Degrees",
-			() -> drivetrain.getPigeon2().getPitch().getValue().in(Units.Degrees),
-			null);
-
-		builder.addDoubleProperty(
-			"Roll Velocity Degrees Per Second",
-			() -> drivetrain
-				.getPigeon2()
-				.getAngularVelocityXDevice()
-				.getValue()
-				.in(Units.DegreesPerSecond),
-			null);
-		builder.addDoubleProperty(
-			"Roll Degrees",
-			() -> drivetrain.getPigeon2().getRoll().getValue().in(Units.Degrees),
-			null);
-
-		addModuleToBuilder(builder, 0);
-		addModuleToBuilder(builder, 1);
-		addModuleToBuilder(builder, 2);
-		addModuleToBuilder(builder, 3);
-	}
-
-	/** Telemeterizes a module */
-	private void addModuleToBuilder(SendableBuilder builder, int module) {
-		TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", 
-			drivetrain.getModules()[module].getDriveMotor(), builder);
-		TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", 
-			drivetrain.getModules()[module].getSteerMotor(), builder);
-	}
+	// @Override
+	// public void initSendable(SendableBuilder builder) {
+	// 	super.initSendable(builder);
+	// 	builder.addDoubleProperty(
+	// 		"Pitch Velocity Degrees Per Second",
+	// 		() -> drivetrain
+	// 			.getPigeon2()
+	// 			.getAngularVelocityYDevice()
+	// 			.getValue()
+	// 			.in(Units.DegreesPerSecond),
+	// 		null);
+	// 	builder.addDoubleProperty(
+	// 		"Pitch Degrees",
+	// 		() -> drivetrain.getPigeon2().getPitch().getValue().in(Units.Degrees),
+	// 		null);
+
+	// 	builder.addDoubleProperty(
+	// 		"Roll Velocity Degrees Per Second",
+	// 		() -> drivetrain
+	// 			.getPigeon2()
+	// 			.getAngularVelocityXDevice()
+	// 			.getValue()
+	// 			.in(Units.DegreesPerSecond),
+	// 		null);
+	// 	builder.addDoubleProperty(
+	// 		"Roll Degrees",
+	// 		() -> drivetrain.getPigeon2().getRoll().getValue().in(Units.Degrees),
+	// 		null);
+
+	// 	addModuleToBuilder(builder, 0);
+	// 	addModuleToBuilder(builder, 1);
+	// 	addModuleToBuilder(builder, 2);
+	// 	addModuleToBuilder(builder, 3);
+	// }
+
+	// /** Telemeterizes a module */
+	// private void addModuleToBuilder(SendableBuilder builder, int module) {
+	// 	TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", 
+	// 		drivetrain.getModules()[module].getDriveMotor(), builder);
+	// 	TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", 
+	// 		drivetrain.getModules()[module].getSteerMotor(), builder);
+	// }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index f9d6c81..d00b8e3 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -8,26 +8,27 @@
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.Time;
+import frc.robot.Constants;
 import frc.robot.lib.control.ControlConstants.*;
 import frc.robot.lib.field.FieldLayout;
-import frc.robot.subsystems.drive.ctre.CtreDriveConstants;
+import frc.robot.subsystems.drive.ctre.CompCtreDriveConstants;
 
 public final class DriveConstants {		
     public static final double EPSILON_TRANSLATION = 0.015; // cm
     public static final double EPSILON_ROTATION = Units.Degrees.of(1.5).in(Units.Radians);
     
     // Maximums
-    public static final double MAX_SPEED = Units.MetersPerSecond.of(4.5).in(Units.MetersPerSecond);
+    public static final double MAX_SPEED = Units.MetersPerSecond.of(4.0).in(Units.MetersPerSecond);
     public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(9.0).in(Units.MetersPerSecondPerSecond);
     public static final double MAX_ROTATION_SPEED = 
-        Units.RotationsPerSecond.of(2.0).in(Units.RadiansPerSecond);
+        Units.RotationsPerSecond.of(1.5).in(Units.RadiansPerSecond);
     public static final double MAX_ROTATION_ACCEL = 
         Units.RotationsPerSecondPerSecond.of(4.0).in(Units.RadiansPerSecondPerSecond);
 
     // Swerve dimensions
     public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters);
     public static final double WHEEL_BASE =	Units.Inches.of(24).in(Units.Meters);
-    public static final double WHEEL_DIAMETER = 2 * CtreDriveConstants.kWheelRadius.in(Units.Meters);
+    public static final double WHEEL_DIAMETER = 2 * CompCtreDriveConstants.kWheelRadius.in(Units.Meters);
     public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI;
 
     // Stability constants
@@ -52,13 +53,11 @@ public final class DriveConstants {
     public static final double ACCELERATION_CONSTANT = 0.1;
 
     public static final double AUTO_ALIGN_TIMEOUT = 0.5;
-
     public static enum FieldPoses {
         HUB(
-            new Pose3d(
-                Units.Inches.of(182.11), 
-                Units.Inches.of(158.84),
-                Units.Inches.of(72), Rotation3d.kZero)),
+            
+            new Pose3d(Constants.FieldConstants.Hub.topCenterPoint, Rotation3d.kZero)
+        ),
         TRENCH(
             FieldLayout.APRILTAG_MAP.getTagPose(12).orElse(Pose3d.kZero)),
         TAG(
@@ -74,4 +73,5 @@ private FieldPoses(Pose3d pose3d) {
             this.pose3d = pose3d;
         }
     }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIO.java b/src/main/java/frc/robot/subsystems/drive/DriveIO.java
new file mode 100644
index 0000000..bf27287
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/DriveIO.java
@@ -0,0 +1,168 @@
+package frc.robot.subsystems.drive;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
+import com.ctre.phoenix6.swerve.SwerveModule;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.lib.io.CancoderIO;
+import frc.robot.lib.io.TalonFXIO;
+import frc.robot.subsystems.drive.ctre.CtreDrive;
+
+public class DriveIO {
+    @AutoLog
+    public static class DriveIOInputs {
+        public Pose2d fieldPose = new Pose2d();
+        public ChassisSpeeds fieldSpeeds = new ChassisSpeeds();
+        public String driveRequest = "";
+
+        @AutoLogOutput
+        public SwerveModuleState[] moduleStates = new SwerveModuleState[4];
+
+        @AutoLogOutput
+        public SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; 
+        
+        @AutoLogOutput
+        public SwerveModuleState[] moduleTargets = new SwerveModuleState[4];
+
+        public String currentCommand = "";
+        public String defaultCommand = "";
+    }
+
+    public class GyroIO {
+        @AutoLog
+        public static class GyroIOInputs {
+            public boolean connected = false;
+            public Rotation2d yawPosition = Rotation2d.kZero;
+            public double yawVelocityRadPerSec = 0.0;
+        }
+
+        private final String name;
+        private final GyroIOInputsAutoLogged inputs;
+        private final StatusSignal<Angle> yaw ;
+        private final StatusSignal<AngularVelocity> yawVelocity;
+
+        public GyroIO(String name, Pigeon2 pigeon) {
+            this.name = name;
+            yaw = pigeon.getYaw();
+            yawVelocity = pigeon.getAngularVelocityZWorld();
+            inputs = new GyroIOInputsAutoLogged();
+        }
+
+        public void updateInputs() {
+            inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
+            inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+            inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
+        }
+
+        public void process() {
+            Logger.processInputs(name, inputs);
+        }
+    }
+
+    public static class ModuleIO {
+        @AutoLog
+        public static class ModuleIOInputs {
+            public SwerveModuleState state;
+            public SwerveModulePosition position; 
+            public SwerveModuleState target;
+        }
+
+        private final String name;
+        private final TalonFXIO driveMotorIO;
+        private final TalonFXIO azimuthMotorIO;
+        private final CancoderIO cancoderIO;
+        private final ModuleIOInputsAutoLogged inputs;
+
+        public ModuleIO(String name, SwerveModule<TalonFX, TalonFX, CANcoder> module) {
+            this.name = name;
+            driveMotorIO = new TalonFXIO(name + "/DriveMotor", module.getDriveMotor());
+            azimuthMotorIO = new TalonFXIO(name + "/AzimuthMotor", module.getSteerMotor());
+            cancoderIO = new CancoderIO(name + "/Cancoder", module.getEncoder());
+            inputs = new ModuleIOInputsAutoLogged();
+        }
+
+        /** Updates the set of loggable inputs. */
+        public void updateInputs(SwerveModuleState state, SwerveModulePosition position, SwerveModuleState target) {
+            inputs.state = state;
+            inputs.position = position;
+            inputs.target = target;
+
+            driveMotorIO.updateInputs();
+            azimuthMotorIO.updateInputs();
+            cancoderIO.updateInputs();
+        }
+
+        public void process() {
+            driveMotorIO.process();
+            azimuthMotorIO.process();
+            cancoderIO.process();
+            Logger.processInputs(name, inputs);
+        }
+    }
+
+    private final String name;
+    private final ModuleIO[] moduleIOs;
+    private final GyroIO gyroIO;
+    private final DriveIOInputsAutoLogged inputs;
+
+    public DriveIO(String name, CtreDrive drivetrain) {
+        this.name = name;
+        inputs = new DriveIOInputsAutoLogged();
+
+        //FrontLeft, FrontRight, BackLeft, BackRight
+        moduleIOs = new ModuleIO[] {
+            new ModuleIO(name + "/Modules/" + "FL", drivetrain.getModule(0)),
+            new ModuleIO(name + "/Modules/" + "FR", drivetrain.getModule(1)),
+            new ModuleIO(name + "/Modules/" + "BL", drivetrain.getModule(2)),
+            new ModuleIO(name + "/Modules/" + "BR", drivetrain.getModule(3))};
+
+        gyroIO = new GyroIO(name + "/Gyro", drivetrain.getPigeon2());
+    }
+
+    public void updateInputs(SwerveRequest request, SwerveDriveState state, Command currentCommand, Command defaultCommand) {
+        inputs.driveRequest = request.getClass().getSimpleName();
+        inputs.fieldPose = state.Pose;
+        inputs.fieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(
+            state.Speeds, state.Pose.getRotation());
+
+        inputs.moduleStates = state.ModuleStates;
+        inputs.modulePositions = state.ModulePositions;
+        inputs.moduleTargets = state.ModuleTargets;
+
+        inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None";
+        inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None";
+        for (int i = 0; i < 4; i++) {
+            moduleIOs[i].updateInputs(state.ModuleStates[i], state.ModulePositions[i], state.ModuleTargets[i]);
+        }
+
+        gyroIO.updateInputs();
+    }
+
+    public void process() {
+        for (int i = 0; i < 4; i++) {
+            moduleIOs[i].process();
+        }
+
+        gyroIO.process();
+        Logger.processInputs(name, inputs);
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/commands/HeadingLockToHub2.java b/src/main/java/frc/robot/subsystems/drive/commands/HeadingLockToHub2.java
new file mode 100644
index 0000000..ebdbb24
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/commands/HeadingLockToHub2.java
@@ -0,0 +1,131 @@
+package frc.robot.subsystems.drive.commands;
+
+import static frc.robot.subsystems.drive.DriveConstants.*;
+
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+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 frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.lib.control.ControlConstants.PIDVConstants;
+import frc.robot.lib.control.ControlConstants.ProfiledPIDVConstants;
+import frc.robot.lib.control.ProfiledPIDVController;
+import frc.robot.lib.util.Util;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.drive.DriveConstants.FieldPoses;
+
+public class HeadingLockToHub2 extends Command {
+    private final Drive drive;
+
+    private final SwerveRequest.FieldCentric request;
+    private final ProfiledPIDVController thetaController;
+
+    private final Translation2d pose;
+
+    private final Debouncer finishDebouncer;
+
+    public HeadingLockToHub2() {
+        this(
+            Constants.FieldConstants.allianceCorrected(
+                FieldPoses.HUB.pose3d.getTranslation())
+                .toTranslation2d());
+    }
+
+	/** 
+	 * Locks the robot onto a pose. 
+	 * Utilizes feedforwards derived from the current chassis speeds
+	 */
+    public HeadingLockToHub2(Translation2d pose) {
+        this(Drive.getInstance(), pose);
+    }
+    
+	/** 
+	 * Locks the robot onto a pose. 
+	 * Utilizes feedforwards derived from the current chassis speeds
+	 */
+	public HeadingLockToHub2(Drive drive, Translation2d pose) {
+        this.pose = pose;
+        this.drive = drive;
+        request = 
+            new SwerveRequest.FieldCentric();
+        
+        thetaController = 
+            new ProfiledPIDVController(
+                new ProfiledPIDVConstants(
+                    new PIDVConstants(10.0, 0.0, 1), 
+                    new TrapezoidProfile.Constraints(Math.PI * 16, Math.PI * 5))
+            );
+        thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+        finishDebouncer = new Debouncer(0.040, DebounceType.kRising);
+        addRequirements(drive);
+    }
+
+    @Override
+    public void initialize() {
+        request.withVelocityX(0).withVelocityY(0)
+            .withRotationalRate(0);
+        drive.setSwerveRequest(request);
+        
+        thetaController.setInitialSetpoint(
+            drive.getPose().getRotation().getRadians(), 
+            drive.getState().Speeds.omegaRadiansPerSecond);
+    }
+    
+    @Override
+    public void execute() {
+        double xDesiredRaw = -Robot.controller.getLeftY();
+        double yDesiredRaw = -Robot.controller.getLeftX();
+
+        double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND);
+        double xFancy = xy[0];
+        double yFancy = xy[1];
+
+        var state = drive.getState();
+        var delta = pose.minus(drive.getPose().getTranslation());
+        var targetDirection = delta.getAngle();
+        
+        var normSq = delta.getNorm() * delta.getNorm();
+        var fieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(state.Speeds, drive.getPose().getRotation());
+        var rotationalRate = normSq > 1e-4 ? 
+            (-delta.getX() * fieldSpeeds.vyMetersPerSecond
+            + delta.getY() * fieldSpeeds.vxMetersPerSecond)
+            / (normSq) : 0.0;
+        
+        var rotation = thetaController
+            .setTarget(targetDirection.getRadians(), rotationalRate)
+            .setMeasurement(state.Pose.getRotation().getRadians(), state.Speeds.omegaRadiansPerSecond)
+            .getOutput();
+
+        SmartDashboard.putNumber("error tracking", 
+            MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180
+        ));
+
+        request
+            // .withHeadingPID(p.get(), i.get(), d.get())
+            .withVelocityX(xFancy * MAX_SPEED)
+            .withVelocityY(yFancy * MAX_SPEED)
+            .withRotationalRate(rotation);
+    }
+	
+    @Override
+    public boolean isFinished() {
+        return finishDebouncer.calculate(
+            MathUtil.isNear(thetaController.getError(), 0, EPSILON_ROTATION)
+        );
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        if (interrupted) {
+            drive.setSwerveRequest(new SwerveRequest.ApplyRobotSpeeds());
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java
index 1cf8f9d..d8cca92 100644
--- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java
+++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java
@@ -81,6 +81,7 @@ public Command withPIDToPoseAtEnd() {
     public void initialize() {
         timer.start(); // actually starts the timer
         drive.setSwerveRequest(request);
+        thetaController.setInitialSetpoint(drive.getPose().getRotation().getRadians(), drive.getFieldSpeeds().omegaRadiansPerSecond);
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java
new file mode 100644
index 0000000..e4f3d89
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java
@@ -0,0 +1,321 @@
+package frc.robot.subsystems.drive.ctre;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.hardware.*;
+import com.ctre.phoenix6.signals.*;
+import com.ctre.phoenix6.swerve.*;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.*;
+import frc.robot.Constants;
+
+// Generated by the 2026 Tuner X Swerve Project Generator
+// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+public class CompCtreDriveConstants {
+    // Both sets of gains need to be tuned to your individual robot.
+
+    // The closed-loop output type to use for the steer motors;
+    // This affects the PID/FF gains for the steer motors
+    public static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+    // The closed-loop output type to use for the drive motors;
+    // This affects the PID/FF gains for the drive motors
+    public static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
+
+    // The type of motor used for the drive motor
+    public static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
+    // The type of motor used for the drive motor
+    public static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
+
+    // The remote sensor feedback type to use for the steer motors;
+    // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
+    public static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+
+    // The stator current at which the wheels start to slip;
+    // This needs to be tuned to your individual robot
+    public static final Current kSlipCurrent = Amps.of(120);
+
+    // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
+    // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
+    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(Amps.of(60))
+                .withSupplyCurrentLimit(40)
+                .withStatorCurrentLimitEnable(true)
+                .withSupplyCurrentLimitEnable(true) 
+        );
+    private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                // Swerve azimuth does not require much torque output, so we can set a relatively low
+                // stator current limit to help avoid brownouts without impacting performance.
+                .withStatorCurrentLimit(Amps.of(40))
+                .withSupplyCurrentLimit(40)
+                .withStatorCurrentLimitEnable(true)
+                .withSupplyCurrentLimitEnable(true)
+        );
+
+    public static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
+    // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
+    public static final Pigeon2Configuration pigeonConfigs = null;
+
+    // CAN bus that the devices are located on;
+    // All swerve devices must share the same CAN bus
+    public static final CANBus kCANBus = new CANBus("CV", "./logs/example.hoot");
+
+    // Theoretical free speed (m/s) at 12 V applied output;
+    // This needs to be tuned to your individual robot
+    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.04);
+
+    // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+    // This may need to be tuned to your individual robot
+    public static final double kCoupleRatio = 3.5714285714285716;
+
+    public static final double kDriveGearRatio = 6.122448979591837;
+    public static final double kSteerGearRatio = 21.428571428571427;
+    public static final Distance kWheelRadius = Inches.of(2);
+
+    public static final boolean kInvertLeftSide = false;
+    public static final boolean kInvertRightSide = true;
+
+    public static final int kPigeonId = 60;
+
+    // These are only used for simulation
+    public static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
+    public static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
+    // Simulated voltage necessary to overcome friction
+    public static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
+    public static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
+
+    
+    // Both sets of gains need to be tuned to your individual robot via sysid.
+
+    // translation sysid results 
+    public static final double kS_sysid_drive = 0.12903;
+    public static final double kV_sysid_drive = 2.3293;
+    public static final double kA_sysid_drive = 0.41181;
+    public static final double kP_sysid_drive = 2.2622;
+    public static final double kD_sysid_drive = 0.0;
+
+    // convert sysid gains into CTRE (rotation/s) units
+    public static final double kS_ctre_drive = kS_sysid_drive;
+    public static final double sysIdToCTRE = Constants.TAU * kWheelRadius.in(Meters) / kDriveGearRatio;
+    public static final double kV_ctre_drive = kV_sysid_drive * sysIdToCTRE;
+    public static final double kA_ctre_drive = kA_sysid_drive * sysIdToCTRE;
+    public static final double kP_ctre_drive = kP_sysid_drive * sysIdToCTRE;
+    public static final double kD_ctre_drive = kD_sysid_drive * sysIdToCTRE;
+
+    // The steer motor uses any SwerveModule.SteerRequestType control request with the
+    // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+    public static final Slot0Configs steerGains = new Slot0Configs()
+            .withKP(100)
+            .withKI(0)
+            .withKD(0.5)
+            .withKS(0.1)
+            .withKV(2.66)
+            .withKA(0)
+            .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+        
+    // When using closed-loop control, the drive motor uses the control
+    // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+    public static final Slot0Configs driveGains = new Slot0Configs()
+            .withKP(kP_ctre_drive)
+            .withKI(0)
+            .withKD(0)
+            .withKS(kS_ctre_drive)
+            .withKV(kV_ctre_drive);
+
+    public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
+            .withCANBusName(kCANBus.getName())
+            .withPigeon2Id(kPigeonId)
+            .withPigeon2Configs(pigeonConfigs);
+
+    public static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
+        new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
+            .withDriveMotorGearRatio(kDriveGearRatio)
+            .withSteerMotorGearRatio(kSteerGearRatio)
+            .withCouplingGearRatio(kCoupleRatio)
+            .withWheelRadius(kWheelRadius)
+            .withSteerMotorGains(steerGains)
+            .withDriveMotorGains(driveGains)
+            .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
+            .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
+            .withSlipCurrent(kSlipCurrent)
+            .withSpeedAt12Volts(kSpeedAt12Volts)
+            .withDriveMotorType(kDriveMotorType)
+            .withSteerMotorType(kSteerMotorType)
+            .withFeedbackSource(kSteerFeedbackType)
+            .withDriveMotorInitialConfigs(driveInitialConfigs)
+            .withSteerMotorInitialConfigs(steerInitialConfigs)
+            .withEncoderInitialConfigs(encoderInitialConfigs)
+            .withSteerInertia(kSteerInertia)
+            .withDriveInertia(kDriveInertia)
+            .withSteerFrictionVoltage(kSteerFrictionVoltage)
+            .withDriveFrictionVoltage(kDriveFrictionVoltage);
+
+
+    // Front Left
+    public static final int kFrontLeftDriveMotorId = 8;
+    public static final int kFrontLeftSteerMotorId = 10;
+    public static final int kFrontLeftEncoderId = 7;
+    public static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.224365234375);
+    public static final boolean kFrontLeftSteerMotorInverted = true;
+    public static final boolean kFrontLeftEncoderInverted = false;
+
+    public static final Distance kFrontLeftXPos = Inches.of(10.875);
+    public static final Distance kFrontLeftYPos = Inches.of(10.875);
+
+    // Front Right
+    public static final int kFrontRightDriveMotorId = 9;
+    public static final int kFrontRightSteerMotorId = 11;
+    public static final int kFrontRightEncoderId = 6;
+    public static final Angle kFrontRightEncoderOffset = Rotations.of(0.431640625);
+    public static final boolean kFrontRightSteerMotorInverted = true;
+    public static final boolean kFrontRightEncoderInverted = false;
+
+    public static final Distance kFrontRightXPos = Inches.of(10.875);
+    public static final Distance kFrontRightYPos = Inches.of(-10.875);
+
+    // Back Left
+    public static final int kBackLeftDriveMotorId = 3;
+    public static final int kBackLeftSteerMotorId = 5;
+    public static final int kBackLeftEncoderId = 14;
+    public static final Angle kBackLeftEncoderOffset = Rotations.of(-0.2119140625);
+    public static final boolean kBackLeftSteerMotorInverted = true;
+    public static final boolean kBackLeftEncoderInverted = false;
+
+    public static final Distance kBackLeftXPos = Inches.of(-10.875);
+    public static final Distance kBackLeftYPos = Inches.of(10.875);
+
+    // Back Right
+    public static final int kBackRightDriveMotorId = 4;
+    public static final int kBackRightSteerMotorId = 2;
+    public static final int kBackRightEncoderId = 1;
+    public static final Angle kBackRightEncoderOffset = Rotations.of(-0.1064453125);
+    public static final boolean kBackRightSteerMotorInverted = true;
+    public static final boolean kBackRightEncoderInverted = false;
+
+    public static final Distance kBackRightXPos = Inches.of(-10.875);
+    public static final Distance kBackRightYPos = Inches.of(-10.875);
+
+
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
+        ConstantCreator.createModuleConstants(
+            kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset,
+            kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
+        ConstantCreator.createModuleConstants(
+            kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
+            kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
+        ConstantCreator.createModuleConstants(
+            kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
+            kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
+        ConstantCreator.createModuleConstants(
+            kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset,
+            kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted
+        );
+
+    /**
+     * Creates a CommandSwerveDrivetrain instance.
+     * This should only be called once in your robot program,.
+     */
+    public static CtreDrive createDrivetrain() {
+        return new CtreDrive(
+            DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
+        );
+    }
+
+
+    /**
+     * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
+     */
+    public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants   Drivetrain-wide constants for the swerve drive
+         * @param modules               Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, modules
+            );
+        }
+
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants     Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency The frequency to run the odometry loop. If
+         *                                unspecified or set to 0 Hz, this is 250 Hz on
+         *                                CAN FD, and 100 Hz on CAN 2.0.
+         * @param modules                 Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, odometryUpdateFrequency, modules
+            );
+        }
+
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants       Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency   The frequency to run the odometry loop. If
+         *                                  unspecified or set to 0 Hz, this is 250 Hz on
+         *                                  CAN FD, and 100 Hz on CAN 2.0.
+         * @param odometryStandardDeviation The standard deviation for odometry calculation
+         *                                  in the form [x, y, theta]áµ€, with units in meters
+         *                                  and radians
+         * @param visionStandardDeviation   The standard deviation for vision calculation
+         *                                  in the form [x, y, theta]áµ€, with units in meters
+         *                                  and radians
+         * @param modules                   Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            Matrix<N3, N1> odometryStandardDeviation,
+            Matrix<N3, N1> visionStandardDeviation,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, odometryUpdateFrequency,
+                odometryStandardDeviation, visionStandardDeviation, modules
+            );
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java
index fc2392f..7e8ba0d 100644
--- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java
+++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java
@@ -4,7 +4,7 @@
 
 import java.util.function.Supplier;
 
-import com.ctre.phoenix6.SignalLogger;
+import org.littletonrobotics.junction.Logger;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
@@ -23,8 +23,13 @@
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
-import frc.robot.subsystems.drive.ctre.CtreDriveConstants.TunerSwerveDrivetrain;
+import frc.robot.subsystems.drive.ctre.CompCtreDriveConstants.TunerSwerveDrivetrain;
 
+
+// FL: -0.419922
+// FR: 0.141113
+// BL: -0.460693
+// BR: 0.193604
 /**
  * Class that extends the Phoenix 6 SwerveDrivetrain class and implements
  * Subsystem so it can easily be used in command-based projects.
@@ -63,21 +68,14 @@ public static enum SysIdRoutineType {
             null,        // Use default timeout (10 s)
             // Log state with SignalLogger class
             // state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
-            null        
+            (state) -> Logger.recordOutput("SysIdDrive", state.toString())        
             ),
         new SysIdRoutine.Mechanism(
             output -> {
                 m_lastAppliedVolts = output.in(Volts);
                 setControl(m_translationCharacterization.withVolts(output));
             },
-            log -> {
-                var s = getStateCopy();
-                log.motor("drive")
-                   .voltage(Volts.of(m_lastAppliedVolts))
-                   .linearPosition(Meters.of(s.Pose.getTranslation().getX())) // or avg wheel distance
-                   .linearVelocity(MetersPerSecond.of(getKinematics()
-                             .toChassisSpeeds(s.ModuleStates).vxMetersPerSecond));
-              },
+            null ,
               this
         )
     );
@@ -89,7 +87,7 @@ public static enum SysIdRoutineType {
             Volts.of(7), // Use dynamic voltage of 7 V
             null,        // Use default timeout (10 s)
             // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
+            (state) -> Logger.recordOutput("SysIdSteer", state.toString())        
         ),
         new SysIdRoutine.Mechanism(
             volts -> setControl(m_steerCharacterization.withVolts(volts)),
@@ -112,7 +110,7 @@ public static enum SysIdRoutineType {
             null, // Use default timeout (10 s)
             // Log state with SignalLogger class
             //state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
-            null
+            (state) -> Logger.recordOutput("SysIdRotation", state.toString())
         ),
         new SysIdRoutine.Mechanism(
             output -> {
@@ -120,14 +118,7 @@ public static enum SysIdRoutineType {
                 m_lastAppliedVolts = output.in(Volts);
                 setControl(m_rotationCharacterization.withRotationalRate(m_lastAppliedVolts));
             },
-            log -> {
-                var s = getStateCopy();
-                log.motor("yaw")
-                   .voltage(Volts.of(m_lastAppliedVolts))
-                   .angularPosition(Radians.of(s.Pose.getRotation().getRadians())) // get rotation position
-                   .angularVelocity(RadiansPerSecond.of(getKinematics()
-                             .toChassisSpeeds(s.ModuleStates).omegaRadiansPerSecond));
-              },
+            null,
               this
            
             // output -> {
diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java
index ce78479..d5c108a 100644
--- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java
@@ -26,7 +26,7 @@
 // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
 public class CtreDriveConstants {
     // which set of robot constants we are deploy 
-    private static final boolean kIs2ndBot = false; //true for the 2nd bot we built for 2026; false for 2025 bot
+    private static final boolean kCompBot = true; //true for the 2nd bot we built for 2026; false for 2025 bot
 
     // mechanical and geometric parameters of drive train 
     public static final double kDriveGearRatio = 6.122448979591837;
@@ -65,7 +65,7 @@ public class CtreDriveConstants {
     private static final Slot0Configs driveGains = new Slot0Configs()
         .withKP(kP_ctre_drive)
         .withKI(0)
-        .withKD(kD_ctre_drive)
+        .withKD(0)
         .withKS(kS_ctre_drive)
         .withKV(kV_ctre_drive);
 
@@ -91,13 +91,18 @@ public class CtreDriveConstants {
 
     // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
     // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
-    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(Amps.of(60))
+                .withStatorCurrentLimitEnable(true)   
+        );
     private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
         .withCurrentLimits(
             new CurrentLimitsConfigs()
                 // Swerve azimuth does not require much torque output, so we can set a relatively low
                 // stator current limit to help avoid brownouts without impacting performance.
-                .withStatorCurrentLimit(Amps.of(60))
+                .withStatorCurrentLimit(Amps.of(40))
                 .withStatorCurrentLimitEnable(true)
         );
     private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
@@ -161,7 +166,7 @@ public class CtreDriveConstants {
     private static final int kFrontLeftDriveMotorId = 8;
     private static final int kFrontLeftSteerMotorId = 10;
     private static final int kFrontLeftEncoderId = 7;
-    private static final Angle kFrontLeftEncoderOffset = Rotations.of(kIs2ndBot ? -0.462890625: 0.355224609375);
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(kCompBot ? -0.419922 : 0.355224609375);
     private static final boolean kFrontLeftSteerMotorInverted = true;
     private static final boolean kFrontLeftEncoderInverted = false;
 
@@ -172,7 +177,7 @@ public class CtreDriveConstants {
     private static final int kFrontRightDriveMotorId = 9;
     private static final int kFrontRightSteerMotorId = 11;
     private static final int kFrontRightEncoderId = 6;
-    private static final Angle kFrontRightEncoderOffset = Rotations.of(kIs2ndBot ? 0.025390625: -0.4296875);
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(kCompBot ? 0.141113 : -0.4296875);
     private static final boolean kFrontRightSteerMotorInverted = true;
     private static final boolean kFrontRightEncoderInverted = false;
 
@@ -183,7 +188,7 @@ public class CtreDriveConstants {
     private static final int kBackLeftDriveMotorId = 3;
     private static final int kBackLeftSteerMotorId = 5;
     private static final int kBackLeftEncoderId = 14;
-    private static final Angle kBackLeftEncoderOffset = Rotations.of(kIs2ndBot ? 0.1435546875: 0.326416015625);
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(kCompBot ? -0.460693 : 0.326416015625);
     private static final boolean kBackLeftSteerMotorInverted = true;
     private static final boolean kBackLeftEncoderInverted = false;
 
@@ -194,7 +199,7 @@ public class CtreDriveConstants {
     private static final int kBackRightDriveMotorId = 4;
     private static final int kBackRightSteerMotorId = 2;
     private static final int kBackRightEncoderId = 1;
-    private static final Angle kBackRightEncoderOffset = Rotations.of(kIs2ndBot ? 0.183837890625: 0.0869140625);
+    private static final Angle kBackRightEncoderOffset = Rotations.of(kCompBot ? 0.193604 : 0.0869140625);
     private static final boolean kBackRightSteerMotorInverted = true;
     private static final boolean kBackRightEncoderInverted = false;
 
diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java
new file mode 100644
index 0000000..b369780
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java
@@ -0,0 +1,293 @@
+package frc.robot.subsystems.drive.ctre;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.hardware.*;
+import com.ctre.phoenix6.signals.*;
+import com.ctre.phoenix6.swerve.*;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.*;
+
+
+// Generated by the 2026 Tuner X Swerve Project Generator
+// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+public class CtreDriveConstants2 {
+    // Both sets of gains need to be tuned to your individual robot.
+
+    // The steer motor uses any SwerveModule.SteerRequestType control request with the
+    // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+    private static final Slot0Configs steerGains = new Slot0Configs()
+        .withKP(60).withKI(0).withKD(0.1)
+        .withKS(0.1).withKV(2.66).withKA(0)
+        .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+    // When using closed-loop control, the drive motor uses the control
+    // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+    private static final Slot0Configs driveGains = new Slot0Configs()
+        .withKP(0.1).withKI(0).withKD(0)
+        .withKS(0).withKV(0.124);
+
+    // The closed-loop output type to use for the steer motors;
+    // This affects the PID/FF gains for the steer motors
+    private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+    // The closed-loop output type to use for the drive motors;
+    // This affects the PID/FF gains for the drive motors
+    private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
+
+    // The type of motor used for the drive motor
+    private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
+    // The type of motor used for the drive motor
+    private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
+
+    // The remote sensor feedback type to use for the steer motors;
+    // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
+    private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+
+    // The stator current at which the wheels start to slip;
+    // This needs to be tuned to your individual robot
+    private static final Current kSlipCurrent = Amps.of(120);
+
+    // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
+    // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
+    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(Amps.of(60))
+                .withStatorCurrentLimitEnable(true)   
+        );
+    private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                // Swerve azimuth does not require much torque output, so we can set a relatively low
+                // stator current limit to help avoid brownouts without impacting performance.
+                .withStatorCurrentLimit(Amps.of(40))
+                .withStatorCurrentLimitEnable(true)
+        );
+    
+    private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
+    // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
+    private static final Pigeon2Configuration pigeonConfigs = null;
+
+    // CAN bus that the devices are located on;
+    // All swerve devices must share the same CAN bus
+    public static final CANBus kCANBus = new CANBus("CV", "./logs/example.hoot");
+
+    // Theoretical free speed (m/s) at 12 V applied output;
+    // This needs to be tuned to your individual robot
+    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.04);
+
+    // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+    // This may need to be tuned to your individual robot
+    
+    private static final double kDriveGearRatio = 6.122448979591837;
+    private static final double kSteerGearRatio = 21.428571428571427;
+    private static final double kCoupleRatio = kDriveGearRatio /(150./7.);
+    // 3.5714285714285716;
+
+    private static final Distance kWheelRadius = Inches.of(2);
+
+    private static final boolean kInvertLeftSide = false;
+    private static final boolean kInvertRightSide = true;
+
+    private static final int kPigeonId = 60;
+
+    // These are only used for simulation
+    private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
+    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
+    // Simulated voltage necessary to overcome friction
+    private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
+    private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
+
+    public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
+            .withCANBusName(kCANBus.getName())
+            .withPigeon2Id(kPigeonId)
+            .withPigeon2Configs(pigeonConfigs);
+
+    private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
+        new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
+            .withDriveMotorGearRatio(kDriveGearRatio)
+            .withSteerMotorGearRatio(kSteerGearRatio)
+            .withCouplingGearRatio(kCoupleRatio)
+            .withWheelRadius(kWheelRadius)
+            .withSteerMotorGains(steerGains)
+            .withDriveMotorGains(driveGains)
+            .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
+            .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
+            .withSlipCurrent(kSlipCurrent)
+            .withSpeedAt12Volts(kSpeedAt12Volts)
+            .withDriveMotorType(kDriveMotorType)
+            .withSteerMotorType(kSteerMotorType)
+            .withFeedbackSource(kSteerFeedbackType)
+            .withDriveMotorInitialConfigs(driveInitialConfigs)
+            .withSteerMotorInitialConfigs(steerInitialConfigs)
+            .withEncoderInitialConfigs(encoderInitialConfigs)
+            .withSteerInertia(kSteerInertia)
+            .withDriveInertia(kDriveInertia)
+            .withSteerFrictionVoltage(kSteerFrictionVoltage)
+            .withDriveFrictionVoltage(kDriveFrictionVoltage);
+
+
+    // Front Left
+    private static final int kFrontLeftDriveMotorId = 8;
+    private static final int kFrontLeftSteerMotorId = 10;
+    private static final int kFrontLeftEncoderId = 7;
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.2236328125);
+    private static final boolean kFrontLeftSteerMotorInverted = true;
+    private static final boolean kFrontLeftEncoderInverted = false;
+
+    private static final Distance kFrontLeftXPos = Inches.of(10.875);
+    private static final Distance kFrontLeftYPos = Inches.of(10.875);
+
+    // Front Right
+    private static final int kFrontRightDriveMotorId = 9;
+    private static final int kFrontRightSteerMotorId = 11;
+    private static final int kFrontRightEncoderId = 6;
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(0.431640625);
+    private static final boolean kFrontRightSteerMotorInverted = true;
+    private static final boolean kFrontRightEncoderInverted = false;
+
+    private static final Distance kFrontRightXPos = Inches.of(10.875);
+    private static final Distance kFrontRightYPos = Inches.of(-10.875);
+
+    // Back Left
+    private static final int kBackLeftDriveMotorId = 3;
+    private static final int kBackLeftSteerMotorId = 5;
+    private static final int kBackLeftEncoderId = 14;
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.210205078125);
+    private static final boolean kBackLeftSteerMotorInverted = true;
+    private static final boolean kBackLeftEncoderInverted = false;
+
+    private static final Distance kBackLeftXPos = Inches.of(-10.875);
+    private static final Distance kBackLeftYPos = Inches.of(10.875);
+
+    // Back Right
+    private static final int kBackRightDriveMotorId = 4;
+    private static final int kBackRightSteerMotorId = 2;
+    private static final int kBackRightEncoderId = 1;
+    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.10693359375);
+    private static final boolean kBackRightSteerMotorInverted = true;
+    private static final boolean kBackRightEncoderInverted = false;
+
+    private static final Distance kBackRightXPos = Inches.of(-10.875);
+    private static final Distance kBackRightYPos = Inches.of(-10.875);
+
+
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
+        ConstantCreator.createModuleConstants(
+            kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset,
+            kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
+        ConstantCreator.createModuleConstants(
+            kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
+            kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
+        ConstantCreator.createModuleConstants(
+            kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
+            kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
+        ConstantCreator.createModuleConstants(
+            kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset,
+            kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted
+        );
+
+    /**
+     * Creates a CommandSwerveDrivetrain instance.
+     * This should only be called once in your robot program,.
+     */
+    public static CtreDrive createDrivetrain() {
+        return new CtreDrive(
+            DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
+        );
+    }
+
+
+    /**
+     * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
+     */
+    public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants   Drivetrain-wide constants for the swerve drive
+         * @param modules               Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, modules
+            );
+        }
+
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants     Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency The frequency to run the odometry loop. If
+         *                                unspecified or set to 0 Hz, this is 250 Hz on
+         *                                CAN FD, and 100 Hz on CAN 2.0.
+         * @param modules                 Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, odometryUpdateFrequency, modules
+            );
+        }
+
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants       Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency   The frequency to run the odometry loop. If
+         *                                  unspecified or set to 0 Hz, this is 250 Hz on
+         *                                  CAN FD, and 100 Hz on CAN 2.0.
+         * @param odometryStandardDeviation The standard deviation for odometry calculation
+         *                                  in the form [x, y, theta]áµ€, with units in meters
+         *                                  and radians
+         * @param visionStandardDeviation   The standard deviation for vision calculation
+         *                                  in the form [x, y, theta]áµ€, with units in meters
+         *                                  and radians
+         * @param modules                   Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            Matrix<N3, N1> odometryStandardDeviation,
+            Matrix<N3, N1> visionStandardDeviation,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, odometryUpdateFrequency,
+                odometryStandardDeviation, visionStandardDeviation, modules
+            );
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/README.md b/src/main/java/frc/robot/subsystems/drive/heatmap/README.md
new file mode 100644
index 0000000..eb65a70
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/heatmap/README.md
@@ -0,0 +1,2 @@
+# 1458Framework2026_2
+A framework for team 1458's 2026 code. Will be using the command-based framework from WPILIB
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/assets/style.css b/src/main/java/frc/robot/subsystems/drive/heatmap/assets/style.css
new file mode 100644
index 0000000..ccc5548
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/heatmap/assets/style.css
@@ -0,0 +1,240 @@
+:root {
+  --bg: #0f1115;
+  --panel: #1c2128;
+  --panel-alt: #242b33;
+  --border: #303a44;
+  --accent: #3d8bfd;
+  --accent-glow: 0 0 0 4px rgba(61, 139, 253, 0.25);
+  --danger: #e55353;
+  --text: #e6edf3;
+  --text-dim: #9da7b3;
+  --radius: 10px;
+  --transition: 160ms ease;
+  --font-stack: "Inter", system-ui, "Segoe UI", Roboto, Arial, sans-serif;
+}
+
+html,
+body {
+  background: var(--bg);
+  color: var(--text);
+  font-family: var(--font-stack);
+  margin: 0;
+  padding: 0;
+}
+
+h1,
+h2,
+h3,
+h4 {
+  font-weight: 600;
+  letter-spacing: 0.5px;
+}
+
+.container {
+  max-width: 1280px;
+  margin: 28px auto 80px;
+  padding: 0 28px;
+}
+
+.toolbar {
+  display: flex;
+  flex-wrap: wrap;
+  gap: 10px;
+  align-items: center;
+  margin-bottom: 14px;
+}
+
+.btn {
+  background: linear-gradient(145deg, var(--panel), var(--panel-alt));
+  color: var(--text);
+  border: 1px solid var(--border);
+  padding: 8px 16px;
+  border-radius: var(--radius);
+  font-size: 14px;
+  font-weight: 500;
+  cursor: pointer;
+  line-height: 1.1;
+  position: relative;
+  transition: var(--transition);
+  display: inline-flex;
+  align-items: center;
+  gap: 6px;
+}
+.btn:hover:not(.active) {
+  border-color: var(--accent);
+  color: #fff;
+}
+.btn:active {
+  transform: translateY(1px);
+}
+.btn.active {
+  background: var(--accent);
+  border-color: var(--accent);
+  color: #fff;
+  box-shadow: var(--accent-glow);
+}
+.btn.danger {
+  background: linear-gradient(145deg, #3a1212, #501b1b);
+  border-color: #6b2222;
+}
+.btn.danger:hover {
+  background: #822828;
+}
+.btn.outline {
+  background: transparent;
+}
+
+.status-label {
+  font-weight: 600;
+  font-size: 14px;
+}
+
+.panel-card {
+  background: var(--panel);
+  border: 1px solid var(--border);
+  border-radius: var(--radius);
+  padding: 14px 18px 24px;
+  position: relative;
+  overflow: hidden;
+}
+.panel-card:before {
+  content: "";
+  position: absolute;
+  inset: 0;
+  pointer-events: none;
+  background: linear-gradient(
+    120deg,
+    rgba(61, 139, 253, 0.12),
+    transparent 35%
+  );
+  opacity: 0.6;
+}
+
+.graph-wrapper {
+  margin-top: 4px;
+}
+
+.footer {
+  margin-top: 42px;
+  padding: 32px 0 60px;
+  font-size: 13px;
+  color: var(--text-dim);
+  text-align: center;
+  border-top: 1px solid var(--border);
+  background: radial-gradient(
+    circle at 50% 10%,
+    rgba(61, 139, 253, 0.08),
+    transparent 60%
+  );
+}
+.footer a {
+  color: var(--accent);
+  text-decoration: none;
+}
+.footer a:hover {
+  text-decoration: underline;
+}
+
+.inline-badge {
+  background: #223041;
+  border: 1px solid #314151;
+  padding: 2px 8px 3px;
+  border-radius: 6px;
+  font-size: 12px;
+  margin-left: 10px;
+  letter-spacing: 0.5px;
+}
+
+/* Kinematics Panel */
+.kinematics-panel {
+  background: var(--panel);
+  border: 1px solid var(--border);
+  border-radius: var(--radius);
+  padding: 12px 18px;
+  margin-bottom: 14px;
+}
+
+.kinematics-header {
+  margin: 0 0 10px 0;
+  font-size: 14px;
+  font-weight: 600;
+  color: var(--text);
+}
+
+.kinematics-inputs {
+  display: flex;
+  flex-wrap: wrap;
+  gap: 16px;
+}
+
+.input-group {
+  display: flex;
+  align-items: center;
+  gap: 8px;
+}
+
+.input-group label {
+  font-size: 13px;
+  color: var(--text-dim);
+  min-width: 100px;
+}
+
+.kinematics-input {
+  background: var(--panel-alt);
+  color: var(--text);
+  border: 1px solid var(--border);
+  border-radius: 6px;
+  padding: 6px 10px;
+  font-size: 14px;
+  width: 90px;
+  transition: var(--transition);
+}
+
+.kinematics-input:focus {
+  outline: none;
+  border-color: var(--accent);
+  box-shadow: var(--accent-glow);
+}
+
+.kinematics-input:hover {
+  border-color: var(--accent);
+}
+
+.input-unit {
+  font-size: 12px;
+  color: var(--text-dim);
+  min-width: 35px;
+}
+
+/* Graph tweaks */
+.js-plotly-plot .plotly .modebar {
+  background: rgba(28, 33, 40, 0.6);
+  border: 1px solid var(--border);
+  border-radius: 6px;
+}
+
+@media (max-width: 860px) {
+  .container {
+    padding: 0 18px;
+  }
+  .toolbar {
+    gap: 8px;
+  }
+  .btn {
+    padding: 8px 14px;
+    font-size: 13px;
+  }
+  .panel-card {
+    padding: 12px 14px 20px;
+  }
+  .kinematics-inputs {
+    flex-direction: column;
+    gap: 10px;
+  }
+  .input-group {
+    width: 100%;
+  }
+  .input-group label {
+    min-width: 110px;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/field.png b/src/main/java/frc/robot/subsystems/drive/heatmap/field.png
new file mode 100644
index 0000000..93be03a
Binary files /dev/null and b/src/main/java/frc/robot/subsystems/drive/heatmap/field.png differ
diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/main.py b/src/main/java/frc/robot/subsystems/drive/heatmap/main.py
new file mode 100644
index 0000000..ad50975
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/heatmap/main.py
@@ -0,0 +1,567 @@
+# app.py
+# Interactive FRC robot cycle-time heatmap with Plotly Dash
+
+import numpy as np
+from heapq import heappush, heappop
+from dataclasses import dataclass
+from typing import List, Tuple, Optional
+import os
+import base64
+import json
+from datetime import datetime
+
+import dash
+from dash import dcc, html, Output, Input, State, ctx
+import plotly.graph_objects as go
+
+#Fetch values from Drive.getinstance().getCtreDrive().getState().Pose?
+
+import ntcore
+
+inst = ntcore.NetworkTableInstance.getDefault()
+table = inst.getTable("SmartDashboard")
+pose_topic = table.getDoubleArrayTopic("Field/Robot")
+subscriber = pose_topic.subscribe([0.0, 0.0, 0.0]) # may debatably swap current x, y order
+
+inst.startClient4("Python-Client") #Python-Monitor
+inst.setServerTeam(1458)
+
+print("Waiting for connection...")
+#while not inst.isConnected():
+ #   x = 0
+
+print("Connected! Streaming Pose:")
+
+try:
+    for i in range(10): # limit infinite loop for testing?
+        pose = subscriber.get()
+        #print(pose)
+        #print(pose[0], pose[1], pose[2]) # cleaner
+except KeyboardInterrupt:
+    pass
+#============================================
+
+def add_paper_background(fig: go.Figure, img_or_data_uri: str, opacity: float = 1.0):
+    """
+    Add a full-plot (paper-aligned) background image.
+    This image does NOT move/scale with axes; it fills the plotting region.
+    """
+    if not img_or_data_uri:
+        return
+    fig.add_layout_image({
+        "source": img_or_data_uri,
+        "xref": "paper", "yref": "paper",
+        "x": 0, "y": 1,
+        "sizex": 1, "sizey": 1,
+        "xanchor": "left", "yanchor": "top",
+        "sizing": "stretch",
+        "layer": "below",
+        "opacity": opacity,
+    })
+
+# -----------------------------
+# Robot Kinematics Constants
+# -----------------------------
+@dataclass
+class RobotKinematics:
+    max_velocity: float = 19.685      # feet per second (6 m/s converted to feet/s)
+    acceleration: float = 13.1234     # feet per second^2 (4 m/s^2 converted to feet/s^2)
+    deceleration: float = 13.1234     # feet per second^2 (4 m/s^2 converted to feet/s^2)
+    turn_time: float = 0.2            # seconds per 90-degree turn (for path changes)
+    
+def distance_to_time(distance_feet: float, kinematics: RobotKinematics = RobotKinematics()) -> float:
+    """
+    Convert distance in feet to time in seconds using robot kinematics.
+    Uses trapezoidal velocity profile: accelerate -> cruise -> decelerate
+    """
+    if distance_feet <= 0:
+        return 0.0
+    
+    # Distance needed to reach max velocity
+    accel_distance = (kinematics.max_velocity ** 2) / (2 * kinematics.acceleration)
+    decel_distance = (kinematics.max_velocity ** 2) / (2 * kinematics.deceleration)
+    
+    # Total distance needed for full acceleration/deceleration
+    min_distance_for_max_speed = accel_distance + decel_distance
+    
+    if distance_feet <= min_distance_for_max_speed:
+        # Triangular profile - never reach max speed
+        # v_max_achieved^2 = 2 * a * d_accel = 2 * a * (distance / 2) = a * distance
+        # Assuming symmetric accel/decel for simplicity
+        avg_accel = (kinematics.acceleration + kinematics.deceleration) / 2
+        v_max_achieved = np.sqrt(avg_accel * distance_feet)
+        time_accel = v_max_achieved / kinematics.acceleration
+        time_decel = v_max_achieved / kinematics.deceleration
+        return time_accel + time_decel
+    else:
+        # Trapezoidal profile - reach max speed
+        time_accel = kinematics.max_velocity / kinematics.acceleration
+        time_decel = kinematics.max_velocity / kinematics.deceleration
+        cruise_distance = distance_feet - accel_distance - decel_distance
+        time_cruise = cruise_distance / kinematics.max_velocity
+        return time_accel + time_cruise + time_decel
+
+# -----------------------------
+# Grid model
+# -----------------------------
+@dataclass
+class GridModel:
+    w: int = 58  # 57 feet 6 7/8 inches ≈ 58 feet
+    h: int = 29  # 26 feet 5 inches ≈ 27 feet
+    robot: Tuple[int, int] = (13, 29)  # row, col (roughly center of field)
+    blocked: List[Tuple[int, int]] = None
+
+    def __post_init__(self):
+        if self.blocked is None:
+            # Example obstacles - you can modify these for actual field elements
+            self.blocked = []
+
+    def is_blocked(self, r: int, c: int) -> bool:
+        return (r, c) in set(self.blocked)
+
+    def toggle_blocked(self, r: int, c: int):
+        if (r, c) == self.robot:
+            return  # do not block the robot cell
+        if (r, c) in self.blocked:
+            self.blocked.remove((r, c))
+        else:
+            self.blocked.append((r, c))
+
+    def set_robot(self, r: int, c: int):
+        if (r, c) in self.blocked:
+            # if user puts robot on blocked cell, un-block it first
+            self.blocked.remove((r, c))
+        self.robot = (r, c)
+
+# -----------------------------
+# Pathfinding (Dijkstra 4-neighbor)
+# -----------------------------
+def dijkstra_distances(w: int, h: int, start: Tuple[int, int], blocked: List[Tuple[int, int]]) -> np.ndarray:
+    blocked_set = set(blocked)
+    sr, sc = start
+    dist = np.full((h, w), np.inf, dtype=float)
+    dist[sr, sc] = 0.0
+    pq = [(0.0, sr, sc)]
+    while pq:
+        d, r, c = heappop(pq)
+        if d > dist[r, c]:
+            continue
+        for dr, dc in [(-1,0),(1,0),(0,-1),(0,1)]:
+            nr, nc = r + dr, c + dc
+            if 0 <= nr < h and 0 <= nc < w and (nr, nc) not in blocked_set:
+                nd = d + 1.0
+                if nd < dist[nr, nc]:
+                    dist[nr, nc] = nd
+                    heappush(pq, (nd, nr, nc))
+    return dist
+
+# -----------------------------
+# Figure builder
+# -----------------------------
+def build_figure(model: GridModel, kinematics: RobotKinematics = None) -> go.Figure:
+    if kinematics is None:
+        kinematics = RobotKinematics()
+    dist = dijkstra_distances(model.w, model.h, model.robot, model.blocked)
+    time_matrix = np.zeros_like(dist, dtype=float)
+    for r in range(model.h):
+        for c in range(model.w):
+            time_matrix[r, c] = distance_to_time(dist[r, c], kinematics) if not np.isinf(dist[r, c]) else np.nan
+
+    viz = time_matrix.copy()
+    for r, c in model.blocked:
+        viz[r, c] = np.nan
+    rr, cc = model.robot
+    viz[rr, cc] = 0.0
+
+    x_vals = list(range(model.w))
+    y_vals = list(range(model.h))
+    zmin = np.nanmin(viz)
+    zmax = np.nanmax(viz)
+
+    heat = go.Heatmap(
+        z=viz,
+        x=x_vals,
+        y=y_vals,
+        colorscale="Viridis",
+        zmin=zmin,
+        zmax=zmax,
+        colorbar=dict(title="Time (s)", thickness=16, len=0.85, x=1.02),
+        hovertemplate="r %{y} • c %{x}<br>%{z:.2f}s<extra></extra>",
+        opacity=0.58,          # transparency so field underneath is visible
+        showscale=True,
+    )
+
+    # Blocked cells (black) drawn above the heat layer
+    mask = np.full_like(viz, np.nan, dtype=float)
+    for r, c in model.blocked:
+        mask[r, c] = 1.0
+    blocked_layer = go.Heatmap(
+        z=mask,
+        x=x_vals,
+        y=y_vals,
+        colorscale=[[0, "black"], [1, "black"]],
+        showscale=False,
+        hoverinfo="skip",
+        zmin=1,
+        zmax=1,
+        opacity=1.0,
+    )
+
+    robot = go.Scatter(
+        x=[cc],
+        y=[rr],
+        mode="markers",
+        marker=dict(symbol="star", size=22, line=dict(color="white", width=2)),
+        name="Robot",
+        hovertemplate="Robot<br>r %{y} c %{x}<extra></extra>",
+        showlegend=False,
+    )
+
+    fig = go.Figure(data=[heat, blocked_layer, robot])
+
+    # Axis ranges (cell edges) and square scaling
+    fig.update_xaxes(visible=False, range=[-0.5, model.w - 0.5], constrain="domain")
+    fig.update_yaxes(visible=False, range=[model.h - 0.5, -0.5], scaleanchor="x", constrain="domain")
+
+    # Paper-aligned subtle background (does not move with pan/zoom)
+    if FIELD_IMAGE_DATA_URI:
+        add_paper_background(fig, FIELD_IMAGE_DATA_URI, opacity=1.0)
+
+    # keep same aspect ratio but scale down
+    orig_w, orig_h = 1405, 652
+    scale = 0.9  # adjust (0.5 = half size, 0.8 = 80%, etc.)
+    fig.update_layout(
+        width=int(orig_w * scale),
+        height=int(orig_h * scale),
+        margin=dict(l=0, r=0, t=0, b=0),
+        paper_bgcolor="#1c2128",
+        plot_bgcolor="#1c2128",
+        font=dict(color="#e6edf3"),
+        showlegend=False,
+        autosize=False,
+    )
+    return fig
+
+# -----------------------------
+# Dash app
+# -----------------------------
+# Explicitly point to the assets folder (resolves path issues when launching from elsewhere)
+ASSETS_PATH = os.path.join(os.path.dirname(__file__), "assets")
+
+app = dash.Dash(
+    __name__,
+    assets_folder=ASSETS_PATH,
+    serve_locally=True,
+)
+
+# Optional: force no cache for dev (uncomment if needed)
+# app.config.update({
+#     "assets_ignore": r"^$",
+#     "serve_locally": True,
+# })
+
+print("Serving assets from:", ASSETS_PATH, "Exists:", os.path.isdir(ASSETS_PATH))
+
+app.title = "FRC Heatmap"
+
+# Path to persist layout
+LAYOUT_SAVE_PATH = os.path.join(os.path.dirname(__file__), "saved_layout.json")
+
+def load_saved_layout() -> Optional[dict]:
+    if os.path.isfile(LAYOUT_SAVE_PATH):
+        try:
+            with open(LAYOUT_SAVE_PATH, "r", encoding="utf-8") as f:
+                data = json.load(f)
+            # minimal validation
+            if all(k in data for k in ("w", "h", "robot", "blocked")):
+                return data
+        except Exception as e:
+            print("Failed to load saved layout:", e)
+    return None
+
+# Stores
+# - model_state: the whole model (robot + blocked)
+# - ui_mode: "robot" or "blocked"
+_saved = load_saved_layout()
+if _saved:
+    try:
+        default_model = GridModel(
+            w=_saved["w"],
+            h=_saved["h"],
+            robot=tuple(_saved["robot"]),
+            blocked=[tuple(x) for x in _saved["blocked"]],
+        )
+        print("Loaded saved layout from disk.")
+    except Exception as e:
+        print("Invalid saved layout, using defaults:", e)
+        default_model = GridModel()
+else:
+    default_model = GridModel()
+
+# Encode field.png once (shown below heatmap)
+FIELD_IMAGE_DATA_URI = None
+_field_path = os.path.join(os.path.dirname(__file__), "field.png")
+if os.path.isfile(_field_path):
+    with open(_field_path, "rb") as _f:
+        FIELD_IMAGE_DATA_URI = "data:image/png;base64," + base64.b64encode(_f.read()).decode()
+else:
+    print("field.png not found; field image below heatmap will be hidden.")
+
+app.layout = html.Div(
+    className="container",
+    children=[
+        html.Link(rel="stylesheet", href="/assets/style.css"),
+        html.H2("FRC Robot Cycle Time Heatmap"),
+        html.P("Interactive visualization of travel time based on drivetrain kinematics and field obstacles."),
+        html.Div(
+            className="toolbar",
+            children=[
+                html.Button("Robot mode", id="btn-robot", n_clicks=0, className="btn"),
+                html.Button("Blocked mode", id="btn-blocked", n_clicks=0, className="btn"),
+                html.Button("Clear obstacles", id="btn-clear", n_clicks=0, className="btn outline"),
+                html.Button("Reset", id="btn-reset", n_clicks=0, className="btn outline"),
+                html.Button("Save layout", id="btn-save", n_clicks=0, className="btn outline"),
+                html.Span(id="mode-label", className="status-label inline-badge"),
+                html.Span(id="save-status", className="status-label", style={"marginLeft": "10px"}),
+            ]
+        ),
+        html.Div(
+            className="kinematics-panel",
+            children=[
+                html.H4("Robot Kinematics", className="kinematics-header"),
+                html.Div(
+                    className="kinematics-inputs",
+                    children=[
+                        html.Div(className="input-group", children=[
+                            html.Label("Max Velocity", htmlFor="input-max-velocity"),
+                            dcc.Input(
+                                id="input-max-velocity",
+                                type="number",
+                                value=19.685,
+                                min=0.1,
+                                max=50,
+                                step=0.1,
+                                className="kinematics-input",
+                            ),
+                            html.Span("ft/s", className="input-unit"),
+                        ]),
+                        html.Div(className="input-group", children=[
+                            html.Label("Acceleration", htmlFor="input-acceleration"),
+                            dcc.Input(
+                                id="input-acceleration",
+                                type="number",
+                                value=13.1234,
+                                min=0.1,
+                                max=50,
+                                step=0.1,
+                                className="kinematics-input",
+                            ),
+                            html.Span("ft/s²", className="input-unit"),
+                        ]),
+                        html.Div(className="input-group", children=[
+                            html.Label("Deceleration", htmlFor="input-deceleration"),
+                            dcc.Input(
+                                id="input-deceleration",
+                                type="number",
+                                value=13.1234,
+                                min=0.1,
+                                max=50,
+                                step=0.1,
+                                className="kinematics-input",
+                            ),
+                            html.Span("ft/s²", className="input-unit"),
+                        ]),
+                        html.Div(className="input-group", children=[
+                            html.Label("Turn Time (90°)", htmlFor="input-turn-time"),
+                            dcc.Input(
+                                id="input-turn-time",
+                                type="number",
+                                value=0.2,
+                                min=0.01,
+                                max=5,
+                                step=0.01,
+                                className="kinematics-input",
+                            ),
+                            html.Span("sec", className="input-unit"),
+                        ]),
+                    ]
+                ),
+            ]
+        ),
+        html.Div(
+            className="panel-card",
+            children=[
+                html.H4("Field Heatmap", style={"marginTop": 0, "marginBottom": "8px"}),
+                dcc.Graph(
+                    id="heatmap",
+                    className="graph-wrapper",
+                    style={"height": "640px", "width": "100%"},
+                    config={"displaylogo": False, "modeBarButtonsToRemove": ["zoom2d","pan2d","lasso2d","select2d"]}
+                ),
+                # Removed standalone field <img> since we now overlay it under the heatmap.
+            ]
+        ),
+        html.Div(
+            className="footer",
+            children=[
+                html.Span("Made for FRC strategy & path planning • "),
+                html.Span("Adjust robot/obstacles by clicking the heatmap. Code uses Dijkstra + kinematic time model."),
+            ]
+        ),
+        dcc.Store(id="model_state", data={
+            "w": default_model.w,
+            "h": default_model.h,
+            "robot": list(default_model.robot),
+            "blocked": [list(p) for p in default_model.blocked],
+        }),
+        dcc.Store(id="ui_mode", data="robot"),
+        dcc.Store(id="kinematics_state", data={
+            "max_velocity": 19.685,
+            "acceleration": 13.1234,
+            "deceleration": 13.1234,
+            "turn_time": 0.2,
+        }),
+    ]
+)
+
+# Update mode label when mode changes
+@app.callback(
+    Output("mode-label", "children"),
+    Input("ui_mode", "data"),
+)
+def show_mode(mode):
+    return f"Current mode: {mode.capitalize()}"
+
+# Mode switching and clear/reset buttons
+@app.callback(
+    Output("ui_mode", "data", allow_duplicate=True),
+    Output("model_state", "data", allow_duplicate=True),
+    Input("btn-robot", "n_clicks"),
+    Input("btn-blocked", "n_clicks"),
+    Input("btn-clear", "n_clicks"),
+    Input("btn-reset", "n_clicks"),
+    State("ui_mode", "data"),
+    State("model_state", "data"),
+    prevent_initial_call=True,
+)
+def on_toolbar(robot_clicks, blocked_clicks, clear_clicks, reset_clicks, mode, data):
+    trigger = ctx.triggered_id
+    model = GridModel(
+        w=data["w"], h=data["h"],
+        robot=tuple(data["robot"]),
+        blocked=[tuple(x) for x in data["blocked"]]
+    )
+    if trigger == "btn-robot":
+        return "robot", data
+    if trigger == "btn-blocked":
+        return "blocked", data
+    if trigger == "btn-clear":
+        model.blocked = []
+        return mode, {"w": model.w, "h": model.h, "robot": list(model.robot), "blocked": [list(p) for p in model.blocked]}
+    if trigger == "btn-reset":
+        model = GridModel()  # back to defaults
+        return "robot", {"w": model.w, "h": model.h, "robot": list(model.robot), "blocked": [list(p) for p in model.blocked]}
+    return dash.no_update, dash.no_update
+
+# Handle clicks on the heatmap: place robot or toggle block
+@app.callback(
+    Output("model_state", "data"),
+    Input("heatmap", "clickData"),
+    State("ui_mode", "data"),
+    State("model_state", "data"),
+    prevent_initial_call=True,
+)
+
+def on_click(click_data, mode, data):
+    if not click_data or "points" not in click_data or not click_data["points"]:
+        return dash.no_update
+    pt = click_data["points"][0]
+    r = int(pose[1]) # non-updating:    r = int(pt["y"])        |        c = int(pt["x"])
+    c = int(pose[0]) # int casting optional (still runs without error); float otherwise
+    #uselessRot = int(pose[2])
+
+    model = GridModel(
+        w=data["w"], h=data["h"],
+        robot=tuple(data["robot"]),
+        blocked=[tuple(x) for x in data["blocked"]]
+    )
+
+    if mode == "robot":
+        model.set_robot(r, c)
+    else:
+        model.toggle_blocked(r, c)
+
+    return {"w": model.w, "h": model.h, "robot": list(model.robot), "blocked": [list(p) for p in model.blocked]}
+
+# Sync kinematics inputs to state store
+@app.callback(
+    Output("kinematics_state", "data"),
+    Input("input-max-velocity", "value"),
+    Input("input-acceleration", "value"),
+    Input("input-deceleration", "value"),
+    Input("input-turn-time", "value"),
+    prevent_initial_call=True,
+)
+def sync_kinematics(max_vel, accel, decel, turn):
+    def safe_float(val, default):
+        try:
+            v = float(val)
+            return v if v > 0 else default
+        except (TypeError, ValueError):
+            return default
+
+    return {
+        "max_velocity": safe_float(max_vel, 19.685),
+        "acceleration": safe_float(accel, 13.1234),
+        "deceleration": safe_float(decel, 13.1234),
+        "turn_time": safe_float(turn, 0.2),
+    }
+
+# Redraw figure whenever the model or kinematics changes
+@app.callback(
+    Output("heatmap", "figure"),
+    Input("model_state", "data"),
+    Input("kinematics_state", "data"),
+)
+def redraw(data, kin_data):
+    model = GridModel(
+        w=data["w"], h=data["h"],
+        robot=tuple(data["robot"]),
+        blocked=[tuple(x) for x in data["blocked"]]
+    )
+    kinematics = RobotKinematics(
+        max_velocity=kin_data["max_velocity"],
+        acceleration=kin_data["acceleration"],
+        deceleration=kin_data["deceleration"],
+        turn_time=kin_data["turn_time"],
+    )
+    return build_figure(model, kinematics)
+
+# Highlight active mode button
+@app.callback(
+    Output("btn-robot", "className"),
+    Output("btn-blocked", "className"),
+    Input("ui_mode", "data"),
+)
+def highlight_active(mode):
+    base = "btn"
+    robot_cls = base + (" active" if mode == "robot" else "")
+    blocked_cls = base + (" active" if mode == "blocked" else "")
+    return robot_cls, blocked_cls
+
+@app.callback(
+    Output("save-status", "children"),
+    Input("btn-save", "n_clicks"),
+    State("model_state", "data"),
+    prevent_initial_call=True,
+)
+def save_layout(n, data):
+    try:
+        with open(LAYOUT_SAVE_PATH, "w", encoding="utf-8") as f:
+            json.dump(data, f, indent=2)
+        return f"Saved {datetime.now().strftime('%H:%M:%S')}"
+    except Exception as e:
+        return f"Save failed: {e}"
+
+if __name__ == "__main__":
+    app.run(debug=True, host='0.0.0.0', port=8050)
diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/saved_layout.json b/src/main/java/frc/robot/subsystems/drive/heatmap/saved_layout.json
new file mode 100644
index 0000000..1d65da8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/heatmap/saved_layout.json
@@ -0,0 +1,282 @@
+{
+  "w": 58,
+  "h": 29,
+  "robot": [
+    8,
+    13
+  ],
+  "blocked": [
+    [
+      23,
+      14
+    ],
+    [
+      23,
+      15
+    ],
+    [
+      23,
+      16
+    ],
+    [
+      23,
+      17
+    ],
+    [
+      23,
+      39
+    ],
+    [
+      23,
+      40
+    ],
+    [
+      23,
+      43
+    ],
+    [
+      23,
+      42
+    ],
+    [
+      23,
+      41
+    ],
+    [
+      23,
+      18
+    ],
+    [
+      5,
+      14
+    ],
+    [
+      5,
+      15
+    ],
+    [
+      5,
+      16
+    ],
+    [
+      5,
+      17
+    ],
+    [
+      5,
+      18
+    ],
+    [
+      5,
+      40
+    ],
+    [
+      5,
+      41
+    ],
+    [
+      5,
+      42
+    ],
+    [
+      5,
+      43
+    ],
+    [
+      5,
+      39
+    ],
+    [
+      16,
+      14
+    ],
+    [
+      15,
+      14
+    ],
+    [
+      14,
+      14
+    ],
+    [
+      13,
+      14
+    ],
+    [
+      12,
+      14
+    ],
+    [
+      12,
+      15
+    ],
+    [
+      12,
+      16
+    ],
+    [
+      12,
+      18
+    ],
+    [
+      12,
+      17
+    ],
+    [
+      16,
+      15
+    ],
+    [
+      16,
+      16
+    ],
+    [
+      16,
+      17
+    ],
+    [
+      16,
+      18
+    ],
+    [
+      15,
+      18
+    ],
+    [
+      14,
+      18
+    ],
+    [
+      13,
+      18
+    ],
+    [
+      16,
+      39
+    ],
+    [
+      15,
+      39
+    ],
+    [
+      14,
+      39
+    ],
+    [
+      12,
+      39
+    ],
+    [
+      13,
+      39
+    ],
+    [
+      12,
+      40
+    ],
+    [
+      12,
+      41
+    ],
+    [
+      12,
+      42
+    ],
+    [
+      12,
+      43
+    ],
+    [
+      13,
+      43
+    ],
+    [
+      14,
+      43
+    ],
+    [
+      15,
+      43
+    ],
+    [
+      16,
+      43
+    ],
+    [
+      16,
+      42
+    ],
+    [
+      16,
+      40
+    ],
+    [
+      16,
+      41
+    ],
+    [
+      15,
+      55
+    ],
+    [
+      15,
+      56
+    ],
+    [
+      15,
+      57
+    ],
+    [
+      15,
+      54
+    ],
+    [
+      12,
+      54
+    ],
+    [
+      12,
+      55
+    ],
+    [
+      12,
+      56
+    ],
+    [
+      12,
+      57
+    ],
+    [
+      14,
+      3
+    ],
+    [
+      14,
+      0
+    ],
+    [
+      14,
+      1
+    ],
+    [
+      14,
+      2
+    ],
+    [
+      17,
+      0
+    ],
+    [
+      17,
+      1
+    ],
+    [
+      17,
+      2
+    ],
+    [
+      17,
+      3
+    ]
+  ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
new file mode 100644
index 0000000..274027f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -0,0 +1,216 @@
+package frc.robot.subsystems.indexer;
+
+import static frc.robot.subsystems.indexer.IndexerConstants.*;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.controls.CoastOut;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Robot;
+
+// TODO (ethan): only activate if shooter ready
+// TODO (ethan): ask tommy setControl(request)
+public class Indexer extends SubsystemBase {
+    /** getInstance of indexer */
+    private static Indexer leftInstance;
+    public static Indexer getLeftInstance() {
+        if (leftInstance == null) {
+            leftInstance = new Indexer(true);
+        }
+        return leftInstance;
+    }
+    private static Indexer rightInstance;
+    public static Indexer getRightInstance() {
+        if (rightInstance == null) {
+            rightInstance = new Indexer(false);
+        }
+        return rightInstance;
+    }
+    private ControlRequest request;
+
+    private TalonFX motor;
+    // private LaserCan lc;
+
+    /** boolean that's modified by checkForBall() */
+    // private boolean hasBall;
+
+    private FlywheelSim wheelSim;
+
+    // private boolean shooterReady = false;
+    // COMMENTED OUT /** boolean that's modified by checkForBallTwo() */
+    // private boolean hasBallTwo;
+
+    /** boolean that's modified by shooter */
+
+    private IndexerIO io;
+
+    /** setup, adding motor and laser */
+    private Indexer(boolean isLeft) {
+        super();
+        setName("Indexer " + (isLeft ? "Left" : "Right"));
+        motor = new TalonFX(isLeft ? L_MOTOR_ID : R_MOTOR_ID);
+        
+        var config = getConfig();
+
+        if (isLeft) {
+            config = config.clone().withMotorOutput(
+                new MotorOutputConfigs()
+                    .withInverted(InvertedValue.Clockwise_Positive));
+        }
+        motor.getConfigurator().apply(config);
+
+        if (Robot.isSimulation()) {
+            wheelSim = new FlywheelSim(
+                LinearSystemId.createFlywheelSystem(
+                    DCMotor.getKrakenX44(1),
+                    0.000189000861,
+                    2), 
+                DCMotor.getKrakenX44(1), 
+                0.0);
+        }
+        io = new IndexerIO(getName(), motor);
+        // TelemetryManager.getInstance().addSendable(this);
+    }
+
+    @Override
+    /* check for balls and makes sure motor is constantly running at desired speed */
+    public void periodic() {
+        motor.setControl(request);
+        
+        // checkForBall();
+        // checkForBallTwo();
+        // if (shooterReady == true) {
+        //     activateIndexer();
+        // }
+        io.updateInputs(motor.getVelocity().getValueAsDouble(), getCurrentCommand(), getDefaultCommand());
+        io.process();
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        
+
+        motor.getSimState().setSupplyVoltage(12);
+
+        wheelSim.setInput(motor.getSimState().getMotorVoltage());
+        wheelSim.update(0.020);
+		motor.getSimState()
+            .setRotorVelocity(wheelSim.getAngularVelocityRPM() / 60.0);
+        motor.getSimState().addRotorPosition(wheelSim.getAngularVelocityRPM() / 60.0 * 0.020);
+    }
+
+    /** type conversion/abstraction */
+    public void setRequest(ControlRequest request) {
+        this.request = request;
+    }
+
+    /** sets speed (duh) */
+    public Command setSpeed(double speed) {
+        return runOnce(() -> setRequest(new VelocityVoltage(speed)));
+    }
+    
+    /** moves motor to speed if sense ball */
+    // public Command loadIndexer() {
+    //     return Commands.either(
+    //         deactivateIndexer(),
+    //         activateIndexer(),
+    //         this::hasBall
+    //     ).repeatedly();
+    // }
+
+    public Command activateIndexer() {
+        return setSpeed(ROLLING_SPEED);
+    }
+
+    public Command back() {
+        return setSpeed(-ROLLING_SPEED);
+    }
+
+    /** turn motor down to zero */
+    public Command deactivateIndexer() {
+        return runOnce(() -> setRequest(new CoastOut()));
+    }
+
+    /** command to sense distance from LaserCAN; used to sense if bol */
+    // private double getDistanceMm() {
+    //     LaserCan.Measurement measurement = lc.getMeasurement();
+    //     if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) {
+    //         return measurement.distance_mm;
+    //     } else {
+    //         return Double.POSITIVE_INFINITY;
+    //     }
+    // }
+
+    /** command that modifies hasBall, uses getDistanceMm() */
+    // private void checkForBall() {
+    //     double x = getDistanceMm();
+    //     if (x <= MAXIMUM_LASER_DIST) {
+    //         hasBall = false;
+    //     }
+    // }
+
+    // /** setup formatting for boolean hasBall so it can be used in activateIndexer().either */
+    // private boolean hasBall() {
+    //     return hasBall;
+    // }
+    // COMMENTED OUT /** setup formatting for boolean hasBall so that it can be used to tell if shooter ready */
+    // private boolean hasBallTwo() {
+    //     return hasBallTwo;
+    // }
+    // COMMENTED OUT /** command to sense if bol going into shooter */
+    // private double getDistanceMmTwo() {
+    //     LaserCan.Measurement measurement = lc.getMeasurement();
+    //     if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) {
+    //         return measurement.distance_mm;
+    //     } else {
+    //         return -1;
+    //     }
+    // }
+    // COMMENTED OUT //** command that checks if bol is boutta be shot */
+    // private void checkForBallTwo() {
+    //     double x = getDistanceMmTwo();
+    //     if (x >= LaserCan_DefaultMeasurement) {
+    //         hasBallTwo = false;
+    //     } else if (x != -1) { /* (x != -1) is checking that the camera isn't just returning an error as ball sensed */
+    //         hasBallTwo = true;
+    //     }
+    // }
+    
+    // TODO: AdvantageKit!
+    /** ????????? */
+    // @Override
+    // public void initSendable(SendableBuilder builder) {
+    //     super.initSendable(builder);
+    //     // builder.addBooleanProperty("Has Ball", () -> hasBall, null);
+    //     TelemetryManager.makeSendableTalonFX("Indexer Motor", motor, builder);
+    // }
+
+    public void runVolts(Voltage voltage) {
+        setRequest(new VoltageOut(voltage));
+    }
+
+    public SysIdRoutine sysId() {
+        return new SysIdRoutine(
+                new SysIdRoutine.Config(
+                        null, null, null, // Use default config
+                        (state) -> Logger.recordOutput("SysIdTestState", state.toString())),
+                new SysIdRoutine.Mechanism(
+                        this::runVolts,
+                        null, // No log consumer, since data is recorded by AdvantageKit
+                        this));
+    }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
new file mode 100644
index 0000000..dc33909
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
@@ -0,0 +1,42 @@
+package frc.robot.subsystems.indexer;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+
+public class IndexerConstants {
+
+    // TODO: Set IDs
+    // TODO (Ethan): Set LaserCan_DefaultMeasurement
+    public static final int L_MOTOR_ID = 26;
+    // public static final int L_LASER_ID = 50;
+    public static final int R_MOTOR_ID = 21;
+    // public static final int R_LASER_ID = 48;
+    // public static final int LASER_ID_2 = 0;
+    public static final double ROLLING_SPEED = 30; // rps
+    public static final double MAXIMUM_LASER_DIST = 100;
+
+
+    public static TalonFXConfiguration getConfig() {
+        return new TalonFXConfiguration()
+            .withSlot0(new Slot0Configs()
+                .withKV(0.0)
+                .withKP(0.3)
+                .withKI(0.0)
+                .withKD(0.0)
+                .withKA(0.021119)
+                .withKS(0.69736)
+                .withKV(0.10261)) // placeholder values
+            .withCurrentLimits(new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(60)
+                .withSupplyCurrentLimit(40)
+                .withStatorCurrentLimitEnable(true)
+                .withSupplyCurrentLimitEnable(true)
+                // .withSupplyCurrentLimit(120)
+                )
+            .withVoltage(new VoltageConfigs()
+                .withPeakForwardVoltage(12.0)
+                .withPeakReverseVoltage(-12.0));
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
new file mode 100644
index 0000000..081878e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
@@ -0,0 +1,41 @@
+package frc.robot.subsystems.indexer;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.lib.io.TalonFXIO;
+
+public class IndexerIO {
+    @AutoLog
+    public static class IndexerIOInputs {
+        public double velocityRPS = 0.0;
+        public String currentCommand = "";
+        public String defaultCommand = "";
+    }
+    
+    private final String name;
+    private final TalonFXIO motorIO;
+    private final IndexerIOInputsAutoLogged inputs;
+
+    public IndexerIO(String name, TalonFX motor) {
+        this.name = name;
+        motorIO = new TalonFXIO(name + "/Motor", motor);
+        inputs = new IndexerIOInputsAutoLogged();
+    }
+
+    public void updateInputs(double velocityRPS, Command currentCommand, Command defaultCommand) {
+        motorIO.updateInputs();
+
+        inputs.velocityRPS = velocityRPS;
+        inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None";
+        inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None";
+    }
+
+    public void process() {
+        motorIO.process();
+        Logger.processInputs(name, inputs);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
new file mode 100644
index 0000000..7d2d6c8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -0,0 +1,250 @@
+package frc.robot.subsystems.intake;
+
+import static frc.robot.subsystems.intake.IntakeConstants.*;
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.controls.CoastOut;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.subsystems.intake.IntakeConstants.Motors;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+
+
+public class Intake extends SubsystemBase {
+    private static Intake intakeInstance;
+
+    public static Intake getInstance() {
+        if (intakeInstance == null) {
+            intakeInstance = new Intake(); 
+        }
+        return intakeInstance;
+    }
+
+    private final TalonFX wheelMotor;
+    private final TalonFX barMotor;
+
+    private double wheelSpeed;
+    private double barPosition;
+
+    private ControlRequest wheelRequest = new NeutralOut();
+    private ControlRequest barRequest = new NeutralOut();
+
+    private SingleJointedArmSim sim;
+    private FlywheelSim wheelSim;
+
+    private IntakeIO io;
+
+    private Intake() {
+        super();
+        wheelMotor = new TalonFX(Motors.WHEEL.id);
+        barMotor = new TalonFX(Motors.BAR.id);
+        wheelMotor.getConfigurator().apply(getWheelConfig());
+		barMotor.getConfigurator().apply(getBarConfig());
+        wheelMotor.setNeutralMode(NeutralModeValue.Coast);
+		barMotor.setNeutralMode(NeutralModeValue.Brake); 
+
+        barMotor.setPosition(BAR_POSITION_UP);
+
+        if (Robot.isSimulation()) {
+            sim = new SingleJointedArmSim(
+                DCMotor.getKrakenX60(1),
+                BAR_GEAR_RATIO,
+                0.1756163, 
+                INTAKE_LENGTH,
+                BAR_POS_MIN,
+                BAR_POS_MAX,
+                true,
+                BAR_POSITION_UP,
+                0.0, 0.0);
+
+            barMotor.getSimState()
+                .setRawRotorPosition(sim.getAngleRads() * (1 / Constants.TAU));
+            
+            wheelSim = new FlywheelSim(
+                LinearSystemId.createFlywheelSystem(
+                    DCMotor.getKrakenX44(1),
+                    0.000189000861,
+                    2), 
+                DCMotor.getKrakenX44(1), 
+                0.0);
+        }
+        
+        io = new IntakeIO(getName(), wheelMotor, barMotor);
+    }
+
+    @Override
+    public void periodic(){
+        wheelSpeed = wheelMotor.getVelocity().getValueAsDouble();
+        barPosition = barMotor.getPosition().getValue().in(Degrees);
+        barMotor.setControl(barRequest);
+        wheelMotor.setControl(wheelRequest);
+        io.updateInputs(wheelSpeed, barPosition, getCurrentCommand(), getDefaultCommand());
+        io.process();
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        barMotor.getSimState().setSupplyVoltage(12.0);
+        sim.setInput(barMotor.getSimState().getMotorVoltage());
+        sim.update(0.020);
+        barMotor.getSimState()
+            .setRotorVelocity(sim.getVelocityRadPerSec() * (1 / Constants.TAU) * BAR_GEAR_RATIO);
+        barMotor.getSimState()
+            .setRawRotorPosition(sim.getAngleRads() * (1 / Constants.TAU) * BAR_GEAR_RATIO);
+
+
+        wheelMotor.getSimState().setSupplyVoltage(12);
+
+        wheelSim.setInput(wheelMotor.getSimState().getMotorVoltage());
+        wheelSim.update(0.020);
+		wheelMotor.getSimState()
+            .setRotorVelocity(wheelSim.getAngularVelocityRPM() / 60.0);
+        wheelMotor.getSimState().addRotorPosition(wheelSim.getAngularVelocityRPM() / 60.0 * 0.020);
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(wheelSim.getCurrentDrawAmps()));
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps()));
+    }
+
+
+    public Command intake() {
+        return setSetpoint(INTAKE_SPEED, BAR_POSITION_DOWN)
+            .andThen(waitUntilBarIsAtPosition(BAR_POSITION_DOWN));
+    }
+
+    public Command outtake() {
+        return setSetpoint(-INTAKE_SPEED, BAR_POSITION_DOWN)
+            .andThen(waitUntilBarIsAtPosition(BAR_POSITION_DOWN));
+    }
+
+    public Command stow() {
+        return setSetpoint(0.0, BAR_POSITION_UP)
+            .andThen(waitUntilBarIsAtPosition(BAR_POSITION_UP));
+    }
+
+    public Command agitate() {
+        return setSetpoint(0.0, BAR_POSITION_DOWN)
+            .andThen(setSetpoint(0.0, BAR_POSITION_MID))
+            .repeatedly();
+    }
+    
+    public Command lower() {
+        return setSetpoint(0.0, BAR_POSITION_DOWN);
+    }
+
+    //----------------set request---------------
+    private void setRequestWheel(ControlRequest request) {
+        this.wheelRequest = request;
+    }
+
+    private void setRequestBar(ControlRequest request) {
+        this.barRequest = request;
+    }
+
+    public Command setSetpoint(double wheelSpeed, double barPosition) {
+        if (Math.abs(wheelSpeed) <= 0.1) {
+            return stopWheel()
+                .andThen(setBarPosition(barPosition)) 
+                .withName("Setpoint: " + wheelSpeed + "rps, " + barPosition + "rot");
+        }
+
+        return 
+            setWheelSpeed(wheelSpeed)
+            .andThen(setBarPosition(barPosition)) 
+            .withName("Setpoint: " + wheelSpeed + "rps, " + barPosition + "rot");
+    }
+
+    //---------bar-----------
+
+    /**
+     * sets request to the bar down voltage
+     * @return
+     */
+    public Command setBarDown() {
+        return setBarPosition(BAR_POSITION_DOWN);
+    }
+
+    /**
+     * sets request to the bar up voltage
+     * @return
+     */
+    public Command setBarUp() {
+        return setBarPosition(BAR_POSITION_UP);
+    }
+    
+    /**
+     * sets position request within limits and then requests that position (in rotations)
+     * @param position
+     * @return
+     */
+    public Command setBarPosition(double position) {
+        double checkedPos = MathUtil.clamp(position, BAR_POS_MIN, BAR_POS_MAX);
+
+        var req = new MotionMagicVoltage(checkedPos);
+        return runOnce(() ->
+            setRequestBar(req)
+        ).withName("bar pos set" + (checkedPos));
+    }
+
+    public Command setWheelSpeed(double speed) {
+        var req = new VelocityVoltage(speed);
+        return runOnce(
+            () -> setRequestWheel(req)
+        ).withName("wheel speed set "+ (speed));
+    }
+
+    public Command stopWheel() {
+        return runOnce(() -> setRequestWheel(new CoastOut()));
+    }
+
+    public Command waitUntilBarIsAtPosition(double target) {
+        return Commands.waitUntil(() -> Math.abs(target - barPosition) < BAR_EPSILON);
+    }
+
+    /**
+     * Recalibrates the elevator zero point. This slowly drives the elevator
+     * down until we see a drop in velocity and a spike in stator current,
+     * indicating that we've hit a hard stop.
+     *
+     * @return Command to run
+     */
+    public Command calibrateZero() {
+        VoltageOut calibrationRequest = new VoltageOut(-0.5)
+            .withIgnoreHardwareLimits(true)
+            .withIgnoreSoftwareLimits(true);
+
+        /** Trigger to detect when the elevator drives into a hard stop. */
+        Trigger isHardStop = new Trigger(() -> {
+            return barMotor.getVelocity().getValue().abs(RotationsPerSecond) < 1 &&
+                barMotor.getTorqueCurrent().getValue().abs(Amps) > 10;
+        }).debounce(0.05);
+
+        return run(() -> {
+            barMotor.setControl(calibrationRequest);
+        })
+        .until(isHardStop)
+        .andThen(
+            runOnce(() -> setRequestBar(new NeutralOut())).withTimeout(0.25)
+                .finallyDo(() -> {
+                    barMotor.setPosition(Rotations.of(0)); 
+                })
+        );
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
new file mode 100644
index 0000000..e5338c9
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -0,0 +1,82 @@
+package frc.robot.subsystems.intake;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Rotations;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import edu.wpi.first.units.Units;
+
+public class IntakeConstants {
+    public static final double BAR_EPSILON = Units.Degrees.of(10).in(Units.Rotations);
+    public static final double INTAKE_SPEED = 70;
+    public static final double BAR_POSITION_DOWN = 0.00;
+    public static final double BAR_POSITION_MID = Degrees.of(60).in(Rotations);
+    public static final double BAR_POSITION_UP = Degrees.of(80).in(Rotations);
+    public static final double BAR_GEAR_RATIO = 44.0 / 18.0 * 5.0 * 4.0;
+    public static final double BAR_POS_MIN = 0.0;
+    public static final double BAR_POS_MAX = Degrees.of(127).in(Rotations);
+    public static final double INTAKE_MASS = 3.656684786; // kg, ideally
+    public static final double INTAKE_LENGTH = 0.1746631508; //m, hopefully
+    
+    public static enum Motors { //TODO: set motor ids; use separate file for ports?
+        WHEEL(31),
+        BAR(32);
+        public final int id;
+        private Motors(int id) {
+            this.id = id;
+        }
+    }
+
+    public static TalonFXConfiguration getWheelConfig() {
+        return new TalonFXConfiguration()
+            .withSlot0(new Slot0Configs()
+                .withKV(0.0)
+                .withKP(1.0)
+                .withKI(0.0)
+                .withKD(0.0))
+            .withCurrentLimits(new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(60)
+                .withSupplyCurrentLimit(40)
+                .withStatorCurrentLimitEnable(true)
+                .withSupplyCurrentLimitEnable(true))
+            .withVoltage(new VoltageConfigs()
+            .withPeakForwardVoltage(12.0)
+            .withPeakReverseVoltage(-12.0))
+            .withMotorOutput(new MotorOutputConfigs()
+                .withInverted(InvertedValue.Clockwise_Positive));        
+    }
+    
+    public static TalonFXConfiguration getBarConfig() { 
+        return new TalonFXConfiguration()
+            .withSlot0(new Slot0Configs()
+                .withKV(0.0)
+                .withKP(20.0)
+                .withKI(0.0)
+                .withKD(0.0)
+                .withKG(0.0).withGravityType(GravityTypeValue.Arm_Cosine))
+            .withCurrentLimits(new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(60)
+                .withSupplyCurrentLimit(40)
+                .withStatorCurrentLimitEnable(true)
+                .withSupplyCurrentLimitEnable(true))
+            .withMotionMagic(new MotionMagicConfigs()
+                .withMotionMagicAcceleration(10) // 1.0 m/s^2
+                .withMotionMagicCruiseVelocity(10)
+                .withMotionMagicJerk(16))
+            .withVoltage(new VoltageConfigs()
+            .withPeakForwardVoltage(12.0)
+            .withPeakReverseVoltage(-12.0))
+            .withFeedback(new FeedbackConfigs()
+                .withSensorToMechanismRatio(BAR_GEAR_RATIO));        
+    }
+    
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
new file mode 100644
index 0000000..0d86905
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -0,0 +1,47 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.lib.io.TalonFXIO;
+
+public class IntakeIO {
+    @AutoLog
+    public static class IntakeIOInputs {
+        public double wheelVelocityRPS = 0.0;
+        public double barPositionDeg = 0.0;
+        public String currentCommand = "";
+        public String defaultCommand = "";
+    }
+
+    private final String name;
+    private final TalonFXIO wheelMotorIO;
+    private final TalonFXIO barMotorIO;
+    private final IntakeIOInputsAutoLogged inputs;
+
+    public IntakeIO(String name, TalonFX wheelMotor, TalonFX barMotor) {
+        this.name = name;
+        wheelMotorIO = new TalonFXIO(name + "/WheelMotor", wheelMotor);
+        barMotorIO = new TalonFXIO(name + "/BarMotor", barMotor);
+        inputs = new IntakeIOInputsAutoLogged();
+    }
+
+    public void updateInputs(double wheelVelocityRPS, double barPositionDeg, Command currentCommand, Command defaultCommand) {
+        wheelMotorIO.updateInputs();
+        barMotorIO.updateInputs();
+
+        inputs.wheelVelocityRPS = wheelVelocityRPS;
+        inputs.barPositionDeg = barPositionDeg;
+        inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None";
+        inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None";
+    }
+
+    public void process() {
+        wheelMotorIO.process();
+        barMotorIO.process();
+        Logger.processInputs(name, inputs);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/led/Led.java b/src/main/java/frc/robot/subsystems/led/Led.java
new file mode 100644
index 0000000..aca4f3c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/Led.java
@@ -0,0 +1,200 @@
+package frc.robot.subsystems.led;
+
+import java.util.Arrays;
+import java.util.function.BiFunction;
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.AddressableLEDSim;
+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.SubsystemBase;
+import frc.robot.Robot;
+
+public class Led extends SubsystemBase {    
+    private static Led ledInstance;
+    public static Led getInstance() {
+        if (ledInstance == null) {
+            ledInstance = new Led();
+        }
+        return ledInstance;
+    }
+
+    /** A helper to optimize LEDs state */
+    private static class Colorer {
+        /** The state of the LEDs */
+        private static enum State {
+            SOLID,
+            PATTERN,
+            TIMED_PATTERN;
+        }
+
+        private final Object lock = new Object();
+        private State state = State.SOLID;
+        private Color solidColor = Color.kGreen;
+        private Color[] pattern;
+        private BiFunction<Integer, Double, Color> timedPatternSupplier;
+        private Color[] colors = new Color[LedConstants.LED_LENGTH];
+        private boolean hasUpdated = true;
+        
+        public void setSolidColor(Color color) {
+            synchronized (lock) {
+                hasUpdated = true;
+                solidColor = color;
+                state = State.SOLID;
+            }
+        }
+
+        public void setPattern(Color[] colors) {
+            synchronized (lock) {
+                hasUpdated = true;
+                pattern = Arrays.stream(colors)
+                    .map((Color c) -> new Color(c.red, c.green, c.blue))
+                    .toArray(Color[]::new);
+                state = State.PATTERN;
+            }
+        }
+
+        public void setTimedPattern(BiFunction<Integer, Double, Color> timedPatternSupplier) {
+            synchronized (lock) {
+                hasUpdated = true;
+                this.timedPatternSupplier = timedPatternSupplier;
+                state = State.TIMED_PATTERN;
+            }
+        }
+
+        /** Gets the colors based on the state */
+        public Color[] get(double time) {
+            synchronized (lock) {
+                switch (state) {
+                    case SOLID: 
+                        if (hasUpdated) {
+                            Arrays.fill(colors, solidColor);
+                        }
+                        break;
+                    case PATTERN:
+                        if (hasUpdated) {
+                            colors = pattern;
+                        }
+                        break;
+                    case TIMED_PATTERN:
+                        for (int i = 0; i < colors.length; i++) {
+                            colors[i] = timedPatternSupplier.apply(i, time);
+                        }
+                        break;
+                    default:
+                        break;
+                };
+                hasUpdated = false;
+                return colors;
+            }
+        }
+    }
+
+    private final Notifier ledNotifier;
+    private final AddressableLED led;
+    private final AddressableLEDBuffer ledBuffer;
+    private final Timer timer;
+    private final Colorer colorer;
+
+    public Led() {
+        ledNotifier = new Notifier(this::update);
+        timer = new Timer();
+        led = new AddressableLED(LedConstants.LED_PORT);
+        ledBuffer = new AddressableLEDBuffer(LedConstants.LED_LENGTH);
+        led.setLength(ledBuffer.getLength());
+        led.start();
+        timer.start();
+        colorer = new Colorer();
+        if (Robot.isSimulation()) {
+            new AddressableLEDSim(led);
+        }
+        ledNotifier.startPeriodic(LedConstants.UPDATE_DT);
+    }
+
+    /** Updates the LEDs */
+    private void update() {
+        if (colorer.state != Colorer.State.TIMED_PATTERN) {
+            if (colorer.hasUpdated) {
+                var colors = colorer.get(timer.get());
+                for(int i = LedConstants.LED_START; i < LedConstants.LED_LENGTH; i++) {
+                    ledBuffer.setLED(i, colors[i]);
+                }
+                led.setData(ledBuffer);
+            }
+        } else {
+            var colors = colorer.get(timer.get());
+            for(int i = LedConstants.LED_START; i < LedConstants.LED_LENGTH; i++) {
+                ledBuffer.setLED(i, colors[i]);
+            }
+            led.setData(ledBuffer);
+        }
+    }    
+    
+    /** Sets the LEDs to a color indefinitely */
+    public Command setSolidColorCommand(Color color) {
+        return setSolidColorCommand(color, Double.POSITIVE_INFINITY);
+    }
+
+    /** Sets the LEDs to a color for a set time */
+    public Command setSolidColorCommand(Color color, double holdTime) {
+        return runOnce(() -> colorer.setSolidColor(color))
+            .andThen(
+                Commands.waitSeconds(holdTime)
+            ).ignoringDisable(true)
+            .withName(color.toString() + ": Solid Color");
+    }
+
+    /** Animates the LEDs with a rainbow animation */
+    public Command setRainbowCommand() {
+        return setRainbowCommand(Double.POSITIVE_INFINITY);
+    }
+
+    /** Sets the LEDs to an animated rainbow for a set time */
+    public Command setRainbowCommand(double holdTime) {
+        return runOnce(() -> colorer.setTimedPattern(
+            (Integer index, Double time) -> 
+                Color.fromHSV(index * 5 + (int) (time * 50), 255, 255)
+        )).andThen(
+            Commands.waitSeconds(holdTime)
+        ).ignoringDisable(true).withName("Rainbow");
+    }
+
+    /** Holds the LEDs at a random state */
+    public Command setRandomCommand() {
+        return setRandomCommand(Double.POSITIVE_INFINITY);
+    }
+
+    /** Sets the LEDs to a random state for a set time */
+    public Command setRandomCommand(double holdTime) {
+        return defer(() -> {
+            Color[] colors = new Color[LedConstants.LED_LENGTH];
+            for (int i = 0; i < colors.length; i++) {
+                colors[i] = new Color(Math.random(), Math.random(), Math.random());
+            }
+            return runOnce(
+                () -> colorer.setPattern(
+                    colors));
+            }
+        ).andThen(
+            Commands.waitSeconds(holdTime)
+        ).ignoringDisable(true).withName("Random");
+    }
+
+    /** Blinks the LEDs indefinitely */
+    public Command blinkCommand(Color color1, Color color2, double delta) {
+        return blinkCommand(color1, color2, delta, Double.POSITIVE_INFINITY);
+    }
+
+    /** Blinks the LEDs for a set time */
+    public Command blinkCommand(Color color1, Color color2, double delta, double holdTime) {
+        return Commands.repeatingSequence(
+            setSolidColorCommand(color1, delta),
+            setSolidColorCommand(color2, delta)
+        ).raceWith(
+            Commands.waitSeconds(holdTime)
+        ).withName(color1.toString() + ":" + color2.toString() + ":" +  (int) (delta * 1000) + " ms Blink");
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/led/LedConstants.java b/src/main/java/frc/robot/subsystems/led/LedConstants.java
new file mode 100644
index 0000000..37fd9a8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/LedConstants.java
@@ -0,0 +1,9 @@
+package frc.robot.subsystems.led;
+
+public class LedConstants {
+    public static final int LED_START = 0;
+    public static final int LED_LENGTH =  19;
+    public static final int LED_LENGTH_2 =  17;
+    public static final int LED_PORT = 7;
+    public static final double UPDATE_DT = 0.06;
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/roller/Roller.java b/src/main/java/frc/robot/subsystems/roller/Roller.java
new file mode 100644
index 0000000..3a8e9a1
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/roller/Roller.java
@@ -0,0 +1,111 @@
+package frc.robot.subsystems.roller;
+
+import static frc.robot.subsystems.roller.RollerConstants.*;
+
+import com.ctre.phoenix6.controls.CoastOut;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+
+
+public class Roller extends SubsystemBase {
+    private static Roller rollerInstance;
+
+    public static Roller getInstance() {
+        if (rollerInstance == null) {
+            rollerInstance = new Roller(); 
+        }
+        return rollerInstance;
+    }
+
+    private final TalonFX motor;
+    private double speed;
+
+    private ControlRequest request = new NeutralOut();
+
+    private FlywheelSim sim;
+    private RollerIO io;
+
+    private Roller() {
+        super();
+        motor = new TalonFX(MOTOR_ID);
+        motor.getConfigurator().apply(getConfig());
+        motor.setNeutralMode(NeutralModeValue.Brake);
+
+        if (Robot.isSimulation()) {
+            sim = new FlywheelSim(
+                LinearSystemId.createFlywheelSystem(
+                    DCMotor.getKrakenX44(1),
+                    0.000189000861,
+                    1), 
+                DCMotor.getKrakenX44(1), 
+                0.0);
+        }
+
+        io = new RollerIO(getName(), motor);
+        // TelemetryManager.getInstance().addSendable(this);
+    }
+
+    @Override
+    public void periodic(){
+        speed = motor.getVelocity().getValueAsDouble();
+        motor.setControl(request);
+        io.updateInputs(speed, getCurrentCommand(), getDefaultCommand());
+        io.process();
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        motor.getSimState().setSupplyVoltage(12);
+        sim.setInput(motor.getSimState().getMotorVoltage());
+        sim.update(0.020);
+		motor.getSimState()
+            .setRotorVelocity(sim.getAngularVelocityRPM() / 60.0);
+        motor.getSimState().addRotorPosition(sim.getAngularVelocityRPM() / 60.0 * 0.020);
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps()));
+    }
+
+    private void setRequest(ControlRequest request) {
+        this.request = request;
+    }
+
+    public Command setSpeed(double speed) {
+        var req = new VelocityVoltage(speed);
+        return runOnce(
+            () -> setRequest(req)
+        ).withName(speed + ":Speed");
+    }
+
+    public Command roll() {
+        return setSpeed(ROLL_SPEED);
+    }
+
+    public Command antiRoll() {
+        return setSpeed(-ROLL_SPEED/2);
+    }
+
+    public Command stop() {
+        return runOnce(() -> setRequest(new CoastOut()))
+            .withName("Stop");
+    }
+
+    // @Override
+    // public void initSendable(SendableBuilder builder){ 
+    //     super.initSendable(builder);
+    //     builder.addDoubleProperty("Speed", () -> speed, null);
+    //     TelemetryManager.makeSendableTalonFX("Roller Motor", motor, builder);
+    // }
+
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java
new file mode 100644
index 0000000..9f968cd
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java
@@ -0,0 +1,35 @@
+package frc.robot.subsystems.roller;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+public class RollerConstants {
+    public static final int MOTOR_ID = 33;
+    public static final double ROLL_SPEED = -10;
+
+    public static TalonFXConfiguration getConfig() {
+        return new TalonFXConfiguration()
+            .withSlot0(new Slot0Configs()
+                .withKV(0.0)
+                .withKP(1.0)
+                .withKI(0.0)
+                .withKD(0.0))
+            .withCurrentLimits(new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(60)
+                .withSupplyCurrentLimit(40)
+                .withStatorCurrentLimitEnable(true)
+                .withSupplyCurrentLimitEnable(true)
+                // .withSupplyCurrentLimit(120)
+                )
+            // .withTorqueCurrent(new TorqueCurrentConfigs()
+            //     .withPeakForwardTorqueCurrent(null))
+            .withVoltage(new VoltageConfigs()
+            .withPeakForwardVoltage(12.0)
+            .withPeakReverseVoltage(-12.0))
+            .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive));        
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIO.java b/src/main/java/frc/robot/subsystems/roller/RollerIO.java
new file mode 100644
index 0000000..c115fbe
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/roller/RollerIO.java
@@ -0,0 +1,41 @@
+package frc.robot.subsystems.roller;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.lib.io.TalonFXIO;
+
+public class RollerIO {
+    @AutoLog
+    public static class RollerIOInputs {
+        public double velocityRPS = 0.0;
+        public String currentCommand = "";
+        public String defaultCommand = "";
+    }
+
+    private final String name;
+    private final TalonFXIO motorIO;
+    private final RollerIOInputsAutoLogged inputs;
+
+    public RollerIO(String name, TalonFX motor) {
+        this.name = name;
+        motorIO = new TalonFXIO(name + "/Motor", motor);
+        inputs = new RollerIOInputsAutoLogged();
+    }
+
+    public void updateInputs(double velocityRPS, Command currentCommand, Command defaultCommand) {
+        motorIO.updateInputs();
+
+        inputs.velocityRPS = velocityRPS;
+        inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None";
+        inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None";
+    }
+
+    public void process() {
+        motorIO.process();
+        Logger.processInputs(name, inputs);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
new file mode 100644
index 0000000..0cd2677
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -0,0 +1,271 @@
+package frc.robot.subsystems.shooter;
+
+import static edu.wpi.first.units.Units.Volts;
+import static frc.robot.subsystems.shooter.ShooterConstants.*;
+
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.controls.CoastOut;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.InterpolatingMatrixTreeMap;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.drive.DriveConstants.FieldPoses;
+import frc.robot.subsystems.shooter.ShooterConstants.Motors;
+
+public class Shooter extends SubsystemBase {
+    private static Shooter shooterLeftInstance;
+    private static Shooter shooterRightInstance;
+
+    public static Shooter getLeftInstance() {
+        if (shooterLeftInstance == null) {
+            shooterLeftInstance = new Shooter(true);
+        }
+        return shooterLeftInstance;
+    }
+
+    public static Shooter getRightInstance() {
+        if (shooterRightInstance == null) {
+            shooterRightInstance = new Shooter(false);
+        }
+        return shooterRightInstance;
+    }
+
+    private final TalonFX topMotor;
+    private final TalonFX bottomMotor;
+    private double lastReadSpeedTop;
+    private double lastReadSpeedBottom;
+    private ControlRequest topRequest = new NeutralOut();
+    private ControlRequest bottomRequest = new NeutralOut();
+
+    private FlywheelSim topSim;
+    private FlywheelSim bottomSim;
+
+    // private ShotCalculator shotCalculator = ShotCalculator.getInstance();
+
+    private InterpolatingMatrixTreeMap<Double, N2, N1> distance_to_shooter = new InterpolatingMatrixTreeMap<Double, N2, N1>();
+    private InterpolatingDoubleTreeMap tree = new InterpolatingDoubleTreeMap();
+
+    private ShooterIO io;
+
+    private Shooter(boolean left) {
+        super();
+
+        setName(this.getClass().getSimpleName() + (left ? "Left" : "Right"));
+
+        int bottomID;
+        int topID;
+
+        if (left) {
+            bottomID = Motors.BOTTOMLEFT.id;
+            topID = Motors.TOPLEFT.id;
+        } else {
+            bottomID = Motors.BOTTOMRIGHT.id;
+            topID = Motors.TOPRIGHT.id;
+        }
+
+        var tconfig = getTopConfig();
+
+        if (!left) {
+            tconfig = tconfig.clone().withMotorOutput(
+                    new MotorOutputConfigs()
+                            .withInverted(InvertedValue.Clockwise_Positive));
+        }
+
+        var bconfig = getBottomConfig();
+
+        if (!left) {
+            bconfig = bconfig.clone().withMotorOutput(
+                    new MotorOutputConfigs()
+                            .withInverted(InvertedValue.Clockwise_Positive));
+        }
+
+        bottomMotor = new TalonFX(bottomID);
+        bottomMotor.getConfigurator().apply(bconfig);
+        bottomMotor.setNeutralMode(NeutralModeValue.Coast);
+
+        topMotor = new TalonFX(topID);
+        topMotor.getConfigurator().apply(tconfig);
+        topMotor.setNeutralMode(NeutralModeValue.Coast);
+
+        if (Robot.isSimulation()) {
+            topSim = new FlywheelSim(
+                    LinearSystemId.createFlywheelSystem(
+                            DCMotor.getKrakenX60(1),
+                            0.000489000861,
+                            1),
+                    DCMotor.getKrakenX60(1), 0.0);
+            bottomSim = new FlywheelSim(
+                    LinearSystemId.createFlywheelSystem(
+                            DCMotor.getKrakenX60(1),
+                            0.000489000861,
+                            1),
+                    DCMotor.getKrakenX60(1), 0.0);
+        }
+
+        io = new ShooterIO(getName(), topMotor, bottomMotor);
+
+        distance_to_shooter.put(0.0, VecBuilder.fill(0, 0));
+        distance_to_shooter.put(0.641, VecBuilder.fill(25.0, 25.0));
+        distance_to_shooter.put(1.06, VecBuilder.fill(31.25, 31.25));
+        distance_to_shooter.put(1.56, VecBuilder.fill(34.375, 34.375));
+        distance_to_shooter.put(2.04, VecBuilder.fill(37.5, 37.5));
+        distance_to_shooter.put(2.54, VecBuilder.fill(49.21875, 49.21875));
+        distance_to_shooter.put(3.0, VecBuilder.fill(60.9375, 60.9375));
+        distance_to_shooter.put(3.4, VecBuilder.fill(81.25, 81.265));
+
+        tree.put(1.24, 27.0);
+        tree.put(1.56, 28.0);
+        tree.put(1.752, 29.8);
+        tree.put(2.005, 32.0);
+        tree.put(2.77, 36.0);
+        tree.put(3.09, 44.0);
+        tree.put(2.45, 34.5);
+        tree.put(2.3, 33.3);
+        tree.put(1.87, 32.0);
+        tree.put(2.094, 32.6);
+        tree.put(2.91, 39.8);
+    }
+
+    @Override
+    public void periodic() {
+        // Read inputs
+        lastReadSpeedTop = topMotor.getVelocity().getValueAsDouble();
+        lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble();
+        topMotor.setControl(topRequest);
+        bottomMotor.setControl(bottomRequest);
+
+        // SmartDashboard.putNumber(
+        // "ShooterV",
+        // distance_to_shooter.get(
+        // SmartDashboard.getNumber("toShooter", 1.5)).get(0, 0));
+
+        io.updateInputs(lastReadSpeedTop, lastReadSpeedBottom, getCurrentCommand(), getDefaultCommand());
+        io.process();
+    }
+
+    public double getTopSpeed() {
+        return lastReadSpeedTop;
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        topMotor.getSimState().setSupplyVoltage(12);
+        bottomMotor.getSimState().setSupplyVoltage(12);
+
+        topSim.setInput(topMotor.getSimState().getMotorVoltage());
+        topSim.update(0.020);
+        topMotor.getSimState()
+                .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0);
+        topMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020);
+        RoboRioSim.setVInVoltage(
+                BatterySim.calculateDefaultBatteryLoadedVoltage(topSim.getCurrentDrawAmps()));
+
+        bottomSim.setInput(bottomMotor.getSimState().getMotorVoltage());
+        bottomSim.update(0.020);
+        RoboRioSim.setVInVoltage(
+                BatterySim.calculateDefaultBatteryLoadedVoltage(bottomSim.getCurrentDrawAmps()));
+        bottomMotor.getSimState()
+                .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0);
+        bottomMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020);
+
+    }
+
+    /** Replaces the request */
+    private void setTopRequest(ControlRequest request) {
+        this.topRequest = request;
+    }
+
+    /** Replaces the request */
+    private void setBottomRequest(ControlRequest request) {
+        this.bottomRequest = request;
+    }
+
+    /** Stops the shooter */
+    public Command stop() {
+        return runOnce(
+                () -> {
+                    setTopRequest(new CoastOut());
+                    setBottomRequest(new CoastOut());
+                }).withName("Stopped");
+    }
+
+    public Command shoot(double topSpeed, double bottomSpeed) {
+        return runOnce(() -> {
+            setTopRequest(new VelocityVoltage(topSpeed));
+            setBottomRequest(new VelocityVoltage(bottomSpeed));
+        }).withName("Shooting");
+    }
+
+    public Command shoot(DoubleSupplier distance) {
+        return shoot(
+            tree.get(distance.getAsDouble()) - 15,
+            tree.get(distance.getAsDouble()) + 15);
+                // distance_to_shooter.get(distance.getAsDouble())
+                //         .get(0, 0) - 15,
+                // distance_to_shooter.get(distance.getAsDouble())
+                //         .get(1, 0) + 15);
+    }
+
+    public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) {
+        var topReq = new VelocityVoltage(0.0);
+        var bottomReq = new VelocityVoltage(0.0);
+        return runOnce(() -> {
+            setTopRequest(topReq);
+            setBottomRequest(bottomReq);
+        }).andThen(
+                run(() -> {
+                    topReq.withVelocity(topSpeed.getAsDouble());
+                    bottomReq.withVelocity(bottomSpeed.getAsDouble());
+                })).withName("Shooting");
+    }
+
+    public Command shoot() {
+        // return shoot(30, 30);
+        return shoot(() -> Drive.getInstance().getPose().getTranslation()
+                .getDistance(
+                        Constants.FieldConstants.allianceCorrected(
+                                FieldPoses.HUB.pose3d.getTranslation()).toTranslation2d()));
+    }
+
+    public void runVolts(Voltage voltage) {
+        setTopRequest(new VoltageOut(voltage));
+    }
+
+    public SysIdRoutine sysId() {
+        return new SysIdRoutine(
+                new SysIdRoutine.Config(
+                        null, null, null, // Use default config
+                        (state) -> Logger.recordOutput("SysIdTestState", state.toString())),
+                new SysIdRoutine.Mechanism(
+                        this::runVolts,
+                        null, // No log consumer, since data is recorded by AdvantageKit
+                        this));
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
new file mode 100644
index 0000000..2afe60e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -0,0 +1,76 @@
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+public final class ShooterConstants {		
+    public static final double GEAR_RATIO = 1;
+
+    public static final double TOPSPIN_FACTOR = 0;
+
+    public static final Transform3d OFFSET = new Transform3d();
+    
+    /** Motor ids */
+    public static enum Motors {
+        TOPLEFT(24),
+        BOTTOMLEFT(25),
+        TOPRIGHT(23),
+        BOTTOMRIGHT(22);
+        public final int id;
+        private Motors(int id) {
+            this.id = id;
+        }
+    }
+    
+    /** Config for shooter motors */
+    public static TalonFXConfiguration getTopConfig() {
+        return new TalonFXConfiguration()
+            .withSlot0(new Slot0Configs()
+                .withKP(0.3)
+                .withKI(0.0)
+                .withKD(0.0)
+                .withKA(0.012289)
+                .withKS(0.12018)
+                .withKV(0.12347)) // placeholder values
+            .withCurrentLimits(new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(60)
+                .withSupplyCurrentLimit(40)
+                .withStatorCurrentLimitEnable(true)
+                .withSupplyCurrentLimitEnable(true))
+            .withVoltage(new VoltageConfigs()
+                .withPeakForwardVoltage(12.0)
+                .withPeakReverseVoltage(-12.0))
+            .withMotionMagic(new MotionMagicConfigs()
+                .withMotionMagicAcceleration(120) // 1.0 m/s^2
+                .withMotionMagicCruiseVelocity(120)
+                .withMotionMagicJerk(120));
+    }
+    /** Config for shooter motors */
+    public static TalonFXConfiguration getBottomConfig() {
+        return new TalonFXConfiguration()
+            .withSlot0(new Slot0Configs()
+                .withKP(0.3)
+                .withKI(0.0)
+                .withKD(0.0)
+                .withKA(0.0092851)
+                .withKS(0.068015)
+                .withKV(0.11781)) // placeholder values
+            .withCurrentLimits(new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(60)
+                .withSupplyCurrentLimit(40)
+                .withStatorCurrentLimitEnable(true)
+                .withSupplyCurrentLimitEnable(true))
+            .withVoltage(new VoltageConfigs()
+                .withPeakForwardVoltage(12.0)
+                .withPeakReverseVoltage(-12.0))
+            .withMotionMagic(new MotionMagicConfigs()
+                .withMotionMagicAcceleration(120) // 1.0 m/s^2
+                .withMotionMagicCruiseVelocity(120)
+                .withMotionMagicJerk(120));
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
new file mode 100644
index 0000000..ceeac54
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
@@ -0,0 +1,47 @@
+package frc.robot.subsystems.shooter;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.lib.io.TalonFXIO;
+
+public  class ShooterIO {
+    @AutoLog
+    public static class ShooterIOInputs {
+        public double topWheelVelocityRPS = 0.0;
+        public double bottomWheelVelocityRPS = 0.0;
+        public String currentCommand = "";
+        public String defaultCommand = "";
+    }
+
+    private final String name;
+    private final TalonFXIO topMotorIO;
+    private final TalonFXIO bottomMotorIO;
+    private final Shooter IOInputsAutoLogged inputs;
+
+    public ShooterIO(String name, TalonFX topMotor, TalonFX bottomMotor) {
+        this.name = name;
+        topMotorIO = new TalonFXIO(name + "/TopMotor", topMotor);
+        bottomMotorIO = new TalonFXIO(name + "/BottomMotor", bottomMotor);
+        inputs = new ShooterIOInputsAutoLogged();
+    }
+
+    public void updateInputs(double topWheelVelocityRPS, double bottomWheelVelocityRPS, Command currentCommand, Command defaultCommand) {
+        topMotorIO.updateInputs();
+        bottomMotorIO.updateInputs();
+
+        inputs.topWheelVelocityRPS = topWheelVelocityRPS;
+        inputs.bottomWheelVelocityRPS = bottomWheelVelocityRPS;
+        inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None";
+        inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None";
+    }
+
+    public void process() {
+        topMotorIO.process();
+        bottomMotorIO.process();
+        Logger.processInputs(name, inputs);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java
new file mode 100644
index 0000000..908ce31
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java
@@ -0,0 +1,87 @@
+// package frc.robot.subsystems.shooter;
+
+// import org.littletonrobotics.junction.AutoLogOutput;
+// import org.littletonrobotics.junction.AutoLogOutputManager;
+// import org.littletonrobotics.junction.Logger;
+
+// import edu.wpi.first.math.geometry.Pose2d;
+// import edu.wpi.first.math.geometry.Pose3d;
+// import edu.wpi.first.math.geometry.Translation3d;
+// import edu.wpi.first.math.kinematics.ChassisSpeeds;
+// import edu.wpi.first.wpilibj2.command.SubsystemBase;
+// import frc.robot.Constants;
+// import frc.robot.lib.houndlib.ShootOnTheFlyCalculator;
+// import frc.robot.lib.houndlib.ShootOnTheFlyCalculator.InterceptSolution;
+// import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels;
+// import frc.robot.subsystems.drive.Drive;
+
+// // stores current target and actively computes effective target
+// public class ShotCalculator extends SubsystemBase {
+//     private static ShotCalculator calcInstance;
+//     public static ShotCalculator getInstance() {
+// 		if (calcInstance == null) {
+//             calcInstance = new ShotCalculator();
+// 		}
+// 		return calcInstance;
+// 	}
+
+//     private final Drive drive;
+
+//     @AutoLogOutput
+//     private Translation3d currentEffectiveTargetPose = Translation3d.kZero;
+//     private double currentEffectiveYaw;
+
+//     @AutoLogOutput
+//     private InterceptSolution currentInterceptSolution;
+//     private Translation3d targetLocation = new Translation3d();
+//     private double targetDistance = 0.0;
+//     private double shooterAngle = 75 * Constants.TAU / 360;
+
+//     private ChassisSpeeds zero = new ChassisSpeeds();
+//     private ChassisAccels zero1 = new ChassisAccels();
+
+//     private ShotCalculator() {
+//         this.drive = Drive.getInstance();
+//         AutoLogOutputManager.addObject(this);
+//     }
+
+//     @Override
+//     public void periodic() {
+//         // Pose2d drivePose = drive.getPose();
+
+//         // targetDistance = drivePose.getTranslation().getDistance(targetLocation.toTranslation2d());
+
+//         // var shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET).getTranslation();
+
+        
+//         // ChassisSpeeds driveSpeeds = drive.getFieldSpeeds();
+//         // ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT);
+
+//         // currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly(
+//         //     shooterPose, 
+//         //     targetLocation,
+//         //     zero, 
+//         //     zero1, 
+//         //     -shooterAngle,
+//         //     5, 0.01);
+
+//         // currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose();
+//         // currentEffectiveYaw = currentInterceptSolution.requiredYaw();
+//     }
+
+//     public void setTarget(Translation3d targetLocation) {
+//         this.targetLocation = targetLocation;
+//     }
+
+//     public Translation3d getCurrentEffectiveTargetPose() {
+//         return currentEffectiveTargetPose;
+//     }
+
+//     public double getCurrentEffectiveYaw() {
+//         return currentEffectiveYaw;
+//     }
+
+//     public InterceptSolution getInterceptSolution() {
+//         return currentInterceptSolution;
+//     }
+// }
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
index f4d64f6..51ec01d 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems.vision;
 
+import static edu.wpi.first.units.Units.Inches;
+
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -18,23 +20,36 @@ public class VisionConstants {
             Math.pow(0.02, 1)); // drive
     public static final Matrix<N3, N1> LOCAL_MEASUREMENT_STD_DEVS =
         VecBuilder.fill(
-            Math.pow(0.2, 1), // vision
-            Math.pow(0.2, 1),
+            Math.pow(0.35, 1), // vision
+            Math.pow(0.35, 1),
             Math.pow(Double.POSITIVE_INFINITY, 1));
+    public static final Matrix<N3, N1> ROTATION_STD_DEVS =
+        VecBuilder.fill(
+            Math.pow(0.35, 1), // vision
+            Math.pow(0.35, 1),
+            Math.pow(0.35, 1));
         
     public static enum VisionDeviceConstants {
         FR_CONSTANTS (
-            "frontr",
+            "right", //right camera 
             new Transform3d(
-                new Translation3d(0.2822, 0.1087, 0.1984),
-                new Rotation3d(0.5 * Constants.TAU, 14.0 * Constants.TAU / 360.0, -26.0 * Constants.TAU/360.0)),
+                new Translation3d(
+                    Inches.of(13.124114),   //wpi x-axis positive is forward direction 
+                    // Inches.of(12.624114),
+                    Inches.of(-9.527904),    //wpi y-axis positive is strafe left, so right camera shall have negative offset 
+                    Inches.of(14.365654)),
+                // new Rotation3d(0, 26 * Constants.TAU / 360.0, -24 * Constants.TAU / 360.0)), //(roll: x, pitch: y, yaw: z)
+                new Rotation3d(0, 26 * Constants.TAU / 360.0, -24 * Constants.TAU / 360.0)),
             1, 1280, 800),
         
         FL_CONSTANTS (
-            "frontl",
+            "left", //left camera 
             new Transform3d(
-                new Translation3d(0.2822, -0.1087, 0.1984),
-                new Rotation3d(0.5 * Constants.TAU, 14.0 * Constants.TAU / 360.0, 26.0 * Constants.TAU/360.0)),
+                new Translation3d(
+                    Inches.of(13.262586),   //wpi x-axis positive is forward direction
+                    Inches.of(7.030256),     //wpi y-axis positive is strafe left, so left camera shall have positive offset
+                    Inches.of(14.325391)),
+                new Rotation3d(0, 26 * Constants.TAU / 360.0, 18 * Constants.TAU / 360.0)),
             2, 1280, 800);
 
         public final String tableName;
@@ -47,6 +62,7 @@ private VisionDeviceConstants(
             Transform3d robotToCamera,
             int cameraId, 
             int cameraResolutionWidth,
+            
             int cameraResolutionHeight
         ) {
             this.tableName = tableName;
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java
index 037bb4a..d7d613c 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java
@@ -14,7 +14,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Robot;
 import frc.robot.lib.field.FieldLayout;
 
@@ -68,10 +67,14 @@ public VisionDevice(VisionDeviceConstants constants) {
 		// poseEstimator = new PhotonPoseEstimator(FieldLayout.APRILTAG_MAP, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, constants.robotToCamera);
 
 		this.constants = constants;
+		TelemetryManager.getInstance().addStructPublisher(
+			constants.name() + "Pose", Pose2d.struct, 
+			() -> botPose);
 
 		hasTarget = false;
 	}
 
+	@SuppressWarnings("removal")
 	private void processFrames() {
 		// var results = camera.getAllUnreadResults();
 		// for (var result : results) {
@@ -82,10 +85,16 @@ private void processFrames() {
 		// 	}
 		// }
 
+		// if (Robot.isSimulation()) {
+		// 	botPose = sim.process(0.01, constants.robotToCamera);
+		// }
 		
 		var result = camera.getLatestResult();
 		if (result.hasTargets()) {
 			var target = result.getBestTarget();
+			if (target.getPoseAmbiguity() > 0.2) {
+				return;
+			}
 
 			var initBotPose = PhotonUtils.estimateFieldToRobotAprilTag(
 				target.getBestCameraToTarget(), 
@@ -127,6 +136,7 @@ private void processFrames() {
 		// }
 	}
 
+	@SuppressWarnings("removal")
 	private void processFramesRigged(Matrix<N3, N1> riggedness) {
 		var result = camera.getLatestResult();
 		if (result.hasTargets()) {
@@ -175,10 +185,10 @@ public void periodic() {
 
 		processFrames();
 
-		SmartDashboard.putNumber(
-				"Vision " + constants.tableName + "/Last Update Timestamp Timestamp", latestTimestamp);
-		// SmartDashboard.putNumber("Vision " + mConstants.tableName + "/N Queued Updates", frames.size());
-		SmartDashboard.putBoolean("Vision " + constants.tableName + "/is Connnected", isConnected);
+		// SmartDashboard.putNumber(
+		// 		"Vision " + constants.tableName + "/Last Update Timestamp Timestamp", latestTimestamp);
+		// // SmartDashboard.putNumber("Vision " + mConstants.tableName + "/N Queued Updates", frames.size());
+		// SmartDashboard.putBoolean("Vision " + constants.tableName + "/is Connnected", isConnected);
 	}
 
 	public boolean isConnected() {
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java
index 535ec67..fd384be 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java
@@ -1,22 +1,16 @@
 package frc.robot.subsystems.vision;
 
-import static frc.robot.subsystems.vision.VisionConstants.*;
-
 import frc.robot.lib.util.TunableNumber;
-import frc.robot.subsystems.TelemetryManager;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.vision.VisionConstants.VisionDeviceConstants;
 import frc.robot.Robot;
 import frc.robot.lib.field.FieldLayout;
 import frc.robot.lib.util.MovingAverageDouble;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import java.util.List;
-import java.util.stream.Collectors;
-
 import org.photonvision.simulation.VisionSystemSim;
 
 public class VisionDeviceManager extends SubsystemBase {
@@ -35,7 +29,7 @@ public static VisionDeviceManager getInstance() {
 	private VisionDevice frontrCamera;
 	private VisionDevice frontlCamera;
 
-	private List<VisionDevice> cameras;
+	public List<VisionDevice> cameras;
 
 	private static TunableNumber timestampOffset = new TunableNumber("VisionTimestampOffset", (0.1), false);
 
@@ -46,6 +40,8 @@ public static VisionDeviceManager getInstance() {
 
 	public VisionSystemSim visionSim;
 
+	public VisionIO io;
+
 	public VisionDeviceManager() {
 		// leftCamera = new VisionDevice(Constants.Limelight.VisionDeviceConstants.L_CONSTANTS);
 		// rightCamera = new VisionDevice(Constants.Limelight.VisionDeviceConstants.R_CONSTANTS);
@@ -58,20 +54,26 @@ public VisionDeviceManager() {
 			visionSim.addAprilTags(FieldLayout.APRILTAG_MAP);
 			cameras.forEach((camera) -> visionSim.addCamera(camera.getSimulation(), camera.getConstants().robotToCamera));
 		}
-		TelemetryManager.getInstance().addSendable(this);
+		io = new VisionIO(getName(), this);
+		// TelemetryManager.getInstance().addSendable(this);
 	}
 
 	@Override
 	public void periodic() {
-		if (Robot.isSimulation()) {
-			visionSim.update(Drive.getInstance().getPose());
-		}
 		cameras.forEach(VisionDevice::periodic);
 		movingAvgRead = headingAvg.getAverage();
-		SmartDashboard.putNumber("Vision heading moving avg", getMovingAvgRead());
-		SmartDashboard.putBoolean("vision disabled", getVisionDisabled());
+		
+		io.updateInputs(getCurrentCommand(), getDefaultCommand());
+		io.process();
+		// SmartDashboard.putNumber("Vision heading moving avg", getMovingAvgRead());
+		// SmartDashboard.putBoolean("vision disabled", getVisionDisabled());
 	}
-
+	
+	@Override
+	public void simulationPeriodic() {
+		visionSim.update(Drive.getInstance().getPose());
+	}
+	
 	public double getMovingAvgRead() {
 		return movingAvgRead;
 	}
@@ -81,16 +83,18 @@ public synchronized MovingAverageDouble getMovingAverage() {
 	}
 
 	public synchronized boolean isFullyConnected() {
-		return frontlCamera.isConnected()
-			&& frontrCamera.isConnected();
+		return true; 
+		// frontlCamera.isConnected()
+		// 	&& frontrCamera.isConnected();
 			// && rightCamera.isConnected();
 			// && backCamera.isConnected();
 	}
 
 	public Command bootUp() {
 		return Commands.parallel(
-			frontlCamera.bootUpSequence(),
-			frontrCamera.bootUpSequence())
+				frontlCamera.bootUpSequence(),
+				frontrCamera.bootUpSequence()
+			)
 			.withTimeout(4)
 			.andThen(Commands.print("Finished vision bootup"));
 	}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
new file mode 100644
index 0000000..916e2b4
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
@@ -0,0 +1,95 @@
+package frc.robot.subsystems.vision;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.List;
+
+public class VisionIO {
+	@AutoLog
+	public static class VisionIOInputs {
+		public boolean enabled = true;
+		public boolean fullyConnected = false;
+
+		public double timestampOffset = 0.0;
+		public double headingMovingAverage = 0.0;
+
+		public String currentCommand = "";
+		public String defaultCommand = "";
+	}
+
+	public static class VisionDeviceIO {
+		@AutoLog
+		public static class VisionDeviceIOInputs {
+			public boolean connected = false;
+			public boolean hasTarget = false;
+			public Pose2d estimatedPose = Pose2d.kZero;
+			public double lastTimestamp = 0.0;
+		}
+
+		private final String name;
+		private final VisionDevice device;
+		private final VisionDeviceIOInputsAutoLogged inputs;
+
+		public VisionDeviceIO(String name, VisionDevice device) {
+			this.name = name;
+			this.device = device;
+			this.inputs = new VisionDeviceIOInputsAutoLogged();
+		}
+
+		public void updateInputs() {
+			inputs.connected = device.isConnected();
+			inputs.hasTarget = device.hasTarget();
+			inputs.estimatedPose = device.botPose != null ? device.botPose : Pose2d.kZero;
+		}
+
+		public void process() {
+			Logger.processInputs(name, inputs);
+		}
+	}
+
+	private final String name;
+	private final VisionDeviceManager manager;
+	private final VisionDeviceIO[] cameraIOs;
+	private final VisionIOInputsAutoLogged inputs;
+
+	public VisionIO(String name, VisionDeviceManager manager) {
+		this.name = name;
+		this.manager = manager;
+		this.inputs = new VisionIOInputsAutoLogged();
+
+		List<VisionDevice> devices = manager.cameras;
+
+		cameraIOs = new VisionDeviceIO[devices.size()];
+		for (int i = 0; i < devices.size(); i++) {
+			cameraIOs[i] = new VisionDeviceIO(
+				name + "/Cameras/" + devices.get(i).getConstants().tableName,
+				devices.get(i));
+		}
+	}
+
+	public void updateInputs(Command currentCommand, Command defaultCommand) {
+		inputs.enabled = !VisionDeviceManager.getVisionDisabled();
+		inputs.timestampOffset = VisionDeviceManager.getTimestampOffset();
+		inputs.fullyConnected = manager.isFullyConnected();
+		inputs.headingMovingAverage = manager.getMovingAvgRead();
+
+		inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None";
+		inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None";
+
+		for (VisionDeviceIO cameraIO : cameraIOs) {
+			cameraIO.updateInputs();
+		}
+	}
+
+	public void process() {
+		for (VisionDeviceIO cameraIO : cameraIOs) {
+			cameraIO.process();
+		}
+
+		Logger.processInputs(name, inputs);
+	}
+}
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 162ad66..5c7ea44 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -4,6 +4,7 @@
     "version": "26.0.0",
     "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
     "frcYear": "2026",
+
     "mavenUrls": [
         "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
     ],
@@ -12,14 +13,18 @@
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-java",
+
             "version": "26.0.0"
+
         }
     ],
     "jniDependencies": [
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-wpilibio",
+
             "version": "26.0.0",
+
             "skipInvalidPlatforms": false,
             "isJar": false,
             "validPlatforms": [