diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json
new file mode 100644
index 0000000..0967ef4
--- /dev/null
+++ b/.DataLogTool/datalogtool.json
@@ -0,0 +1 @@
+{}
diff --git a/.SysId/sysid.json b/.SysId/sysid.json
new file mode 100644
index 0000000..0967ef4
--- /dev/null
+++ b/.SysId/sysid.json
@@ -0,0 +1 @@
+{}
diff --git a/src/main/deploy/pathplanner/autos/90 Degree Test Auto.auto b/src/main/deploy/pathplanner/autos/90 Degree Test Auto.auto
deleted file mode 100644
index 891a8e9..0000000
--- a/src/main/deploy/pathplanner/autos/90 Degree Test Auto.auto	
+++ /dev/null
@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "HP to hub and shoot"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/HP Auto + Climb With Auto Aim + Auto Climb.auto b/src/main/deploy/pathplanner/autos/Center Auto.auto
similarity index 71%
rename from src/main/deploy/pathplanner/autos/HP Auto + Climb With Auto Aim + Auto Climb.auto
rename to src/main/deploy/pathplanner/autos/Center Auto.auto
index 3d32169..9521ee9 100644
--- a/src/main/deploy/pathplanner/autos/HP Auto + Climb With Auto Aim + Auto Climb.auto	
+++ b/src/main/deploy/pathplanner/autos/Center Auto.auto	
@@ -10,28 +10,28 @@
             "name": "Deploy_Intake_Command"
           }
         },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Starting to HP"
-          }
-        },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.5
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "HP to Hub"
-          }
-        },
         {
           "type": "named",
           "data": {
-            "name": "Auto_Aim_To_Hub"
+            "name": "Start_Intake_Wheels"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center to Floor Intake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Floor Intake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Floor Intake to Left Shoot"
           }
         },
         {
@@ -43,25 +43,25 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 3.0
+            "waitTime": 2.0
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "Kill_All"
+            "name": "Assist_Shooter"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "PathPlan_To_Climb_Right_Offsetted"
+            "pathName": "Left Shoot to Offsetted-Left Climb"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "PathPlan_Into_Climb_Right"
+            "pathName": "Offsetted-Left to Left Climb"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/HP to Hub and Shoot Auto.auto b/src/main/deploy/pathplanner/autos/HP to Hub and Shoot Auto.auto
deleted file mode 100644
index ce599cd..0000000
--- a/src/main/deploy/pathplanner/autos/HP to Hub and Shoot Auto.auto	
+++ /dev/null
@@ -1,31 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "named",
-          "data": {
-            "name": "Deploy_Intake_Command"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "HP to hub and shoot"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Shoot_Fuel_Command"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto b/src/main/deploy/pathplanner/autos/Human Player Auto.auto
similarity index 91%
rename from src/main/deploy/pathplanner/autos/HP Auto + Climb.auto
rename to src/main/deploy/pathplanner/autos/Human Player Auto.auto
index 1db36d4..22e1e55 100644
--- a/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto	
+++ b/src/main/deploy/pathplanner/autos/Human Player Auto.auto	
@@ -19,7 +19,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 1.5
+            "waitTime": 1.25
           }
         },
         {
@@ -37,7 +37,7 @@
         {
           "type": "wait",
           "data": {
-            "waitTime": 1.0
+            "waitTime": 2.0
           }
         },
         {
@@ -46,12 +46,6 @@
             "name": "Assist_Shooter"
           }
         },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 2.0
-          }
-        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/Left Side + Center Fuel Auto.auto b/src/main/deploy/pathplanner/autos/Left Side + Center Fuel Auto.auto
new file mode 100644
index 0000000..83e4c95
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Left Side + Center Fuel Auto.auto	
@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to Over Left Ramp"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Deploy_Intake_Command"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Start_Intake_Wheels"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Collect Left-Center Fuel"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Collect Left-Center Fuel to Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot_Fuel_Command"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Assist_Shooter"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left Shoot to Offsetted-Left Climb"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Offsetted-Left to Left Climb"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift_Robot"
+          }
+        }
+      ]
+    }
+  },
+  "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 33c18ce..cc7bc57 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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,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,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,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
+{"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,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,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,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,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/Center to Floor Intake.path b/src/main/deploy/pathplanner/paths/Center to Floor Intake.path
new file mode 100644
index 0000000..b926fd4
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Center to Floor Intake.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.528863636363636,
+        "y": 5.086295454545454
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.539647727272727,
+        "y": 5.818079545454545
+      },
+      "isLocked": false,
+      "linkedName": "Middle Start Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 1.1789090909090907,
+        "y": 5.941761363636363
+      },
+      "prevControl": {
+        "x": 2.9929090909090905,
+        "y": 5.261511363636364
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Left Intake Balls Waypoint"
+    }
+  ],
+  "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": 180.0
+  },
+  "reversed": false,
+  "folder": "Middle Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Collect Left-Center Fuel to Shoot.path b/src/main/deploy/pathplanner/paths/Collect Left-Center Fuel to Shoot.path
new file mode 100644
index 0000000..2b236b9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Collect Left-Center Fuel to Shoot.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.734045454548223,
+        "y": 4.6843295454545455
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.147034090909091,
+        "y": 5.282125000000001
+      },
+      "isLocked": false,
+      "linkedName": "Collect Fuel Left Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 2.6837045454545447,
+        "y": 4.591568181818182
+      },
+      "prevControl": {
+        "x": 2.6837045454545447,
+        "y": 6.805519750430038
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Floor Intake to Shoot Waypoint"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6572327044025181,
+      "rotationDegrees": -91.02718603450556
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -18.80579011375007
+  },
+  "reversed": false,
+  "folder": "Left Auto",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Collect Left-Center Fuel.path b/src/main/deploy/pathplanner/paths/Collect Left-Center Fuel.path
new file mode 100644
index 0000000..bfdcfdd
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Collect Left-Center Fuel.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.734045454548223,
+        "y": 7.054897727274547
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.734045454545454,
+        "y": 5.075988636363636
+      },
+      "isLocked": false,
+      "linkedName": "Over Left Ramp Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 7.734045454548223,
+        "y": 4.6843295454545455
+      },
+      "prevControl": {
+        "x": 7.734045454548223,
+        "y": 6.8982811140664015
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Collect Fuel Left Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Left Auto",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Floor Intake to Left Shoot.path b/src/main/deploy/pathplanner/paths/Floor Intake to Left Shoot.path
new file mode 100644
index 0000000..5df5471
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Floor Intake to Left Shoot.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.5192727272727271,
+        "y": 5.941761363636363
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6630909090909087,
+        "y": 4.591568181818182
+      },
+      "isLocked": false,
+      "linkedName": "Floor Intake Fuel Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 2.6837045454545447,
+        "y": 4.591568181818182
+      },
+      "prevControl": {
+        "x": 0.8490909090909089,
+        "y": 5.745931818181819
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Floor Intake to Shoot Waypoint"
+    }
+  ],
+  "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": -18.80579011375007
+  },
+  "reversed": false,
+  "folder": "Middle Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Floor Intake.path b/src/main/deploy/pathplanner/paths/Floor Intake.path
new file mode 100644
index 0000000..d191b91
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Floor Intake.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1789090909090907,
+        "y": 5.941761363636363
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.5089659090909093,
+        "y": 5.931454545454544
+      },
+      "isLocked": false,
+      "linkedName": "Left Intake Balls Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 0.5192727272727271,
+        "y": 5.941761363636363
+      },
+      "prevControl": {
+        "x": 0.9521590909090447,
+        "y": 5.9314545454499195
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Floor Intake Fuel Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Middle Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/HP to Hub.path b/src/main/deploy/pathplanner/paths/HP to Hub.path
index 70c66d8..4aa3c02 100644
--- a/src/main/deploy/pathplanner/paths/HP to Hub.path	
+++ b/src/main/deploy/pathplanner/paths/HP to Hub.path	
@@ -3,16 +3,16 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.447,
+        "x": 0.46,
         "y": 0.634
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.8005533905932738,
-        "y": 0.9875533905932738
+        "x": 0.8135533905932739,
+        "y": 0.9875533905932734
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "HP Waypoint"
     },
     {
       "anchor": {
@@ -25,7 +25,7 @@
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Right Shoot Position Waypoint"
     }
   ],
   "rotationTargets": [],
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 27.207
+    "rotation": 36.0
   },
   "reversed": false,
-  "folder": "Start to HP to Hub to Climb",
+  "folder": "HP to Shoot to Climb",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Hub to side of Climb.path b/src/main/deploy/pathplanner/paths/Hub to side of Climb.path
index ff4a8e6..03534a5 100644
--- a/src/main/deploy/pathplanner/paths/Hub to side of Climb.path	
+++ b/src/main/deploy/pathplanner/paths/Hub to side of Climb.path	
@@ -12,20 +12,20 @@
         "y": 2.3289899283371653
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Right Shoot Position Waypoint"
     },
     {
       "anchor": {
-        "x": 1.25,
-        "y": 2.0457840909090907
+        "x": 1.14,
+        "y": 2.046
       },
       "prevControl": {
-        "x": 1.7198463103929542,
-        "y": 2.2167941625719254
+        "x": 1.6098463103929541,
+        "y": 2.2170100716628345
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Offsetted-Climb Waypoint"
     }
   ],
   "rotationTargets": [],
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -99.66394495351781
+    "rotation": -90.0
   },
   "reversed": false,
-  "folder": "Start to HP to Hub to Climb",
+  "folder": "HP to Shoot to Climb",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 27.207
+    "rotation": 36.0
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Left Shoot to Offsetted-Left Climb.path b/src/main/deploy/pathplanner/paths/Left Shoot to Offsetted-Left Climb.path
new file mode 100644
index 0000000..15d70dd
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Left Shoot to Offsetted-Left Climb.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.6837045454545447,
+        "y": 4.591568181818182
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.4468863636391924,
+        "y": 4.911079545452217
+      },
+      "isLocked": false,
+      "linkedName": "Floor Intake to Shoot Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 1.14,
+        "y": 5.0
+      },
+      "prevControl": {
+        "x": 2.467261363639192,
+        "y": 4.653409090906762
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Ofsetted-Left Climb Waypoint"
+    }
+  ],
+  "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": "Middle Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -18.80579011375007
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Hub to Shoot.path b/src/main/deploy/pathplanner/paths/Left to Floor Intake.path
similarity index 72%
rename from src/main/deploy/pathplanner/paths/Hub to Shoot.path
rename to src/main/deploy/pathplanner/paths/Left to Floor Intake.path
index 055f568..7df7ec6 100644
--- a/src/main/deploy/pathplanner/paths/Hub to Shoot.path	
+++ b/src/main/deploy/pathplanner/paths/Left to Floor Intake.path	
@@ -4,28 +4,28 @@
     {
       "anchor": {
         "x": 3.5185568181818176,
-        "y": 4.024693181818182
+        "y": 7.219806818181819
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.879534090909091,
-        "y": 4.024693181818182
+        "x": 1.5190340909090907,
+        "y": 6.12728409090909
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.281738636363636,
-        "y": 4.024693181818182
+        "x": 1.1789090909090907,
+        "y": 5.941761363636363
       },
       "prevControl": {
-        "x": 2.8279999999999994,
-        "y": 4.024693181818182
+        "x": 2.9929090909090905,
+        "y": 6.910602272727273
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Left Intake Balls Waypoint"
     }
   ],
   "rotationTargets": [],
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 180.0
   },
   "reversed": false,
   "folder": "Middle Shoot to Climb",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 180.0
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Side of Climb to Climb.path b/src/main/deploy/pathplanner/paths/Offsetted-Left to Left Climb.path
similarity index 69%
rename from src/main/deploy/pathplanner/paths/Side of Climb to Climb.path
rename to src/main/deploy/pathplanner/paths/Offsetted-Left to Left Climb.path
index cf2120f..8de2a7e 100644
--- a/src/main/deploy/pathplanner/paths/Side of Climb to Climb.path	
+++ b/src/main/deploy/pathplanner/paths/Offsetted-Left to Left Climb.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.8,
-        "y": 2.0
+        "x": 1.14,
+        "y": 5.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.8,
-        "y": 2.5
+        "x": 1.14,
+        "y": 4.292625343835472
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Ofsetted-Left Climb Waypoint"
     },
     {
       "anchor": {
-        "x": 0.8,
-        "y": 2.7
+        "x": 1.14,
+        "y": 4.25
       },
       "prevControl": {
-        "x": 0.8,
-        "y": 2.2
+        "x": 1.14,
+        "y": 4.70494908797556
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 1.0,
-    "maxAcceleration": 1.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -90.0
+    "rotation": 90.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Middle Shoot to Climb",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -90.0
+    "rotation": 90.0
   },
   "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Side into Climb.path b/src/main/deploy/pathplanner/paths/Side into Climb.path
index fb59d7b..a012d9b 100644
--- a/src/main/deploy/pathplanner/paths/Side into Climb.path	
+++ b/src/main/deploy/pathplanner/paths/Side into Climb.path	
@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.25,
+        "x": 1.14,
         "y": 2.046
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.302590909090909,
-        "y": 2.6126590909090908
+        "x": 1.1561466905187832,
+        "y": 2.7126296958855094
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Offsetted-Right Climb Waypoint"
     },
     {
       "anchor": {
-        "x": 1.37,
-        "y": 3.5
+        "x": 1.14,
+        "y": 3.25
       },
       "prevControl": {
-        "x": 1.3761477272727272,
-        "y": 3.190795454545455
+        "x": 1.146147727272727,
+        "y": 2.940795454545455
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Right Climb Waypoint"
     }
   ],
   "rotationTargets": [],
@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 0.5,
+    "maxVelocity": 1.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -99.664
+    "rotation": -90.0
   },
   "reversed": false,
-  "folder": "Start to HP to Hub to Climb",
+  "folder": "HP to Shoot to Climb",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -99.664
+    "rotation": -90.0
   },
   "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/HP to hub and shoot.path b/src/main/deploy/pathplanner/paths/Start to Over Left Ramp.path
similarity index 53%
rename from src/main/deploy/pathplanner/paths/HP to hub and shoot.path
rename to src/main/deploy/pathplanner/paths/Start to Over Left Ramp.path
index 986ce14..fc03880 100644
--- a/src/main/deploy/pathplanner/paths/HP to hub and shoot.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Over Left Ramp.path	
@@ -3,62 +3,53 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.4655813953488368,
-        "y": 0.6682289803220032
+        "x": 3.5185568181818176,
+        "y": 7.2095
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.580858676207512,
-        "y": -0.27284436493738906
+        "x": 1.1273750000027687,
+        "y": 5.385193181820003
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.591109123434704,
-        "y": 2.1771914132379244
+        "x": 4.672920454548223,
+        "y": 5.477954545456365
       },
       "prevControl": {
-        "x": 3.078571154198153,
-        "y": 2.162419836548123
+        "x": 3.3539345896947985,
+        "y": 5.471848129415376
       },
       "nextControl": {
-        "x": 2.055670840787119,
-        "y": 2.1934168157423963
+        "x": 6.899193181820951,
+        "y": 5.488261363638184
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.077871198568873,
-        "y": 3.86463327370304
+        "x": 7.734045454548223,
+        "y": 7.054897727274547
       },
       "prevControl": {
-        "x": 1.9420930232558136,
-        "y": 4.026887298747764
+        "x": 5.971579545454545,
+        "y": 7.271340909090909
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.5,
-      "rotationDegrees": 180.0
-    },
-    {
-      "waypointRelativePos": 1.567134268537075,
-      "rotationDegrees": -1.0
+      "linkedName": "Over Left Ramp Waypoint"
     }
   ],
+  "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.0,
+    "maxVelocity": 4.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -67,13 +58,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -90.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Auto",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -90.0
   },
   "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Starting to HP.path b/src/main/deploy/pathplanner/paths/Starting to HP.path
index 123cd2c..4d0ed5e 100644
--- a/src/main/deploy/pathplanner/paths/Starting to HP.path	
+++ b/src/main/deploy/pathplanner/paths/Starting to HP.path	
@@ -4,28 +4,28 @@
     {
       "anchor": {
         "x": 3.5159570661896247,
-        "y": 0.4572987477638639
+        "y": 0.8
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.8344901610017894,
-        "y": 0.4248479427549193
+        "x": 2.6424772727272727,
+        "y": 0.7471249999999998
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Start Right Trench Waypoint"
     },
     {
       "anchor": {
-        "x": 0.457431818181818,
-        "y": 0.63375
+        "x": 0.46,
+        "y": 0.634
       },
       "prevControl": {
-        "x": 1.0093340606142802,
-        "y": 0.6337499999999999
+        "x": 1.0119022424324617,
+        "y": 0.6340000000000003
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "HP Waypoint"
     }
   ],
   "rotationTargets": [],
@@ -45,7 +45,7 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": "Start to HP to Hub to Climb",
+  "folder": "HP to Shoot to Climb",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0
diff --git a/src/main/deploy/pathplanner/paths/test_1_Meter.path b/src/main/deploy/pathplanner/paths/test_1_Meter.path
index d799800..a8840ff 100644
--- a/src/main/deploy/pathplanner/paths/test_1_Meter.path
+++ b/src/main/deploy/pathplanner/paths/test_1_Meter.path
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 15.474,
-        "y": 3.437204545454546
+        "x": 1.0,
+        "y": 1.87
       },
       "prevControl": null,
       "nextControl": {
-        "x": 16.473999999999986,
-        "y": 3.437204545454546
+        "x": 1.9999999999999858,
+        "y": 1.87
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.066,
+        "x": 3.0,
         "y": 1.87
       },
       "prevControl": {
-        "x": -0.07677934128731523,
-        "y": 1.87
+        "x": 1.8572206587126847,
+        "y": 1.8700000000000003
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 90.0
+    "rotation": 0.0
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 90.0
+    "rotation": 0.0
   },
   "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 2dc50a1..08192a6 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -3,8 +3,9 @@
   "robotLength": 0.9,
   "holonomicMode": true,
   "pathFolders": [
+    "HP to Shoot to Climb",
     "Middle Shoot to Climb",
-    "Start to HP to Hub to Climb"
+    "Left Auto"
   ],
   "autoFolders": [],
   "defaultMaxVel": 3.0,
diff --git a/src/main/deploy/swerve/neo/modules/backright.json b/src/main/deploy/swerve/neo/modules/backright.json
index 95b169c..c9795c4 100644
--- a/src/main/deploy/swerve/neo/modules/backright.json
+++ b/src/main/deploy/swerve/neo/modules/backright.json
@@ -3,7 +3,7 @@
     "front": -10.9,
     "left": -10.9
   },
-  "absoluteEncoderOffset": 287.578125,
+  "absoluteEncoderOffset": 283.7109375,
   "drive": {
     "type": "sparkflex_neo",
     "id": 11,
diff --git a/src/main/deploy/swerve/neo/modules/frontleft.json b/src/main/deploy/swerve/neo/modules/frontleft.json
index add715d..cc1d4ee 100644
--- a/src/main/deploy/swerve/neo/modules/frontleft.json
+++ b/src/main/deploy/swerve/neo/modules/frontleft.json
@@ -3,7 +3,7 @@
     "front": 10.9,
     "left": 10.9
   },
-  "absoluteEncoderOffset": 36.03515625,
+  "absoluteEncoderOffset": 34.716796875,
   "drive": {
     "type": "sparkflex_neo",
     "id": 13,
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index ed69bb0..2820777 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -74,6 +74,8 @@ public final class Constants {
         //        .withWidget(BuiltInWidgets.kNumberBar).getEntry();
 
         public static double SHOOTER_RPM = -2700;
+        public static final double IDLE_SHOOTER_RPM = -1800;
+        public static final double AUTO_SHOOTER_RPM = -3100; //always negative to shoot
 
        // public static void updateShooterRPM() {
           //  SHOOTER_RPM = shooterRPM.getDouble(-4000);
@@ -100,7 +102,7 @@ public final class Constants {
          * .withWidget(BuiltInWidgets.kNumberBar).getEntry();
          */
 
-        public static double INDEXER_AND_RAMP_MOTOR_RPM = 30000;
+        public static double INDEXER_AND_RAMP_MOTOR_RPM = 15000; //always positive when feeding into shooter
 
         // this method called in robot periodic so values updated in elastic are
         // constantly read and applied to RAMP_MOTOR_SPEED
@@ -118,7 +120,9 @@ public final class Constants {
          * programmingTab.add("Desired Intake RPM", -1000)
          * .withWidget(BuiltInWidgets.kNumberBar).getEntry();
          */
-        public static double INTAKE_WHEELS_MOTOR_RPM = -6250;
+        public static double INTAKE_WHEELS_MOTOR_RPM_FAST = -8000; // always negative when intaking
+        public static double INTAKE_WHEELS_MOTOR_RPM_SLOW = -5000; // always negative when intaking
+
 
         /*
          * public static void updateIntakeWheelsRPM() {
@@ -130,36 +134,36 @@ public final class Constants {
         public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
 
         public static class IntakeRotatorPID {
-            public static final double INTAKE_ROTATOR_P = .08;
+            public static final double INTAKE_ROTATOR_P = .07;
             public static final double INTAKE_ROTATOR_I = 0;
-            public static final double INTAKE_ROTATOR_D = 0.01;
+            public static final double INTAKE_ROTATOR_D = 0.03;
         }
 
         public static final double INTAKE_MOTOR_P = 0.0001;
         public static final double INTAKE_MOTOR_I = 0;
-        public static final double INTAKE_MOTOR_D = 0;
+        public static final double INTAKE_MOTOR_D = 0.00005;
 
-        public static final double INTAKE_COLLECT_ENCODER_VALUE = 1.4;
-        public static final double INTAKE_MIDDLE_ENCODER_VALUE = -3;
-        public static final double INTAKE_RETRACT_ENCODER_VALUE = -5.5;
+        public static final double INTAKE_COLLECT_ENCODER_VALUE = 1;
+        public static final double INTAKE_MIDDLE_ENCODER_VALUE = -4;
+        public static final double INTAKE_RETRACT_ENCODER_VALUE = -6.2;
 
         public static final double INTAKE_THROUGHBORE_ENCODER_DEPLOY = .13;
         public static final double INTAKE_THROUGHBORE_ENCODER_RETRACT = .49;
         public static final double INTAKE_THROUGHBORE_ENCODER_MIDDLE = .389;
 
-        public static final double INTAKE_MANUAL_SPEED = .1;
+        public static final double INTAKE_MANUAL_SPEED = .15;
     }
 
     // create object and a new widget under programming tab in Elastic where object
     // retrieves value from widget
 
     public static class TargetingConstants {
-        public static final Pose2d BLUE_LEFT_CLIMB_POSE = new Pose2d(1.066, 4.633, Rotation2d.fromDegrees(90));
-        public static final Pose2d BLUE_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(1.066, 5.133,
+        public static final Pose2d BLUE_LEFT_CLIMB_POSE = new Pose2d(1.14, 4.25, Rotation2d.fromDegrees(90));
+        public static final Pose2d BLUE_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(1.14, 5.0,
                 Rotation2d.fromDegrees(90));
 
-        public static final Pose2d BLUE_RIGHT_CLIMB_POSE = new Pose2d(1.066, 2.87, Rotation2d.fromDegrees(-90));
-        public static final Pose2d BLUE_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(1.066, 2.37,
+        public static final Pose2d BLUE_RIGHT_CLIMB_POSE = new Pose2d(1.14, 2.046, Rotation2d.fromDegrees(-90));
+        public static final Pose2d BLUE_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(1.14, 3.25,
                 Rotation2d.fromDegrees(-90));
 
         public static final Pose2d RED_RIGHT_CLIMB_POSE = new Pose2d(15.474, 5.22, Rotation2d.fromDegrees(90));
@@ -198,12 +202,12 @@ public final class Constants {
                 new Rotation3d(0, 0, Math.toRadians(-200)));
 
         public static final Transform3d ROBOT_TO_FRONT_LEFT_CAM = new Transform3d(
-                new Translation3d(Units.inchesToMeters(23.375), 0, Units.inchesToMeters(13)),
-                new Rotation3d(0, 74, 0));
+                new Translation3d(Units.inchesToMeters(1.3), Units.inchesToMeters(11.25), Units.inchesToMeters(26)),
+                new Rotation3d(0, Math.toRadians(10), 0));
 
         public static final Transform3d ROBOT_TO_FRONT_RIGHT_CAM = new Transform3d(
-                new Translation3d(0, 0, 0),
-                new Rotation3d(0, 0, 0));
+                new Translation3d(Units.inchesToMeters(1.5), Units.inchesToMeters(-11.25), Units.inchesToMeters(26)),
+                new Rotation3d(0, Math.toRadians(-10), 0));
     }
 
     public static class ClimberConstants {
@@ -215,7 +219,7 @@ public final class Constants {
         public static final double CLIMBER_PID_I = 0;
         public static final double CLIMBER_PID_D = 0;
 
-        public static final double CLIMBER_LIFTED_SETPOINT_VALUE = 45;
+        public static final double CLIMBER_LIFTED_SETPOINT_VALUE = 65;
         public static final double CLIMBER_LOWERED_SETPOINT_VALUE = 0;
 
     }
diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java
deleted file mode 100644
index ded212c..0000000
--- a/src/main/java/frc/robot/LimelightHelpers.java
+++ /dev/null
@@ -1,1947 +0,0 @@
-//LimelightHelpers v1.14 (REQUIRES LLOS 2026.0 OR LATER)
-
-package frc.robot;
-
-import edu.wpi.first.networktables.DoubleArrayEntry;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.TimestampedDoubleArray;
-import frc.robot.LimelightHelpers.LimelightResults;
-import frc.robot.LimelightHelpers.PoseEstimate;
-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.Translation3d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation2d;
-
-import java.net.MalformedURLException;
-import java.net.URL;
-import java.util.Arrays;
-import java.util.Map;
-
-import com.fasterxml.jackson.annotation.JsonFormat;
-import com.fasterxml.jackson.annotation.JsonFormat.Shape;
-import com.fasterxml.jackson.annotation.JsonProperty;
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.DeserializationFeature;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import java.util.concurrent.ConcurrentHashMap;
-import edu.wpi.first.net.PortForwarder;
-
-/**
- * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC.
- * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking.
- */
-public class LimelightHelpers {
-
-    private static final Map<String, DoubleArrayEntry> doubleArrayEntries = new ConcurrentHashMap<>();
-
-    /**
-     * Represents a Color/Retroreflective Target Result extracted from JSON Output
-     */
-    public static class LimelightTarget_Retro {
-
-        @JsonProperty("t6c_ts")
-        private double[] cameraPose_TargetSpace;
-
-        @JsonProperty("t6r_fs")
-        private double[] robotPose_FieldSpace;
-
-        @JsonProperty("t6r_ts")
-        private  double[] robotPose_TargetSpace;
-
-        @JsonProperty("t6t_cs")
-        private double[] targetPose_CameraSpace;
-
-        @JsonProperty("t6t_rs")
-        private double[] targetPose_RobotSpace;
-
-        public Pose3d getCameraPose_TargetSpace()
-        {
-            return toPose3D(cameraPose_TargetSpace);
-        }
-        public Pose3d getRobotPose_FieldSpace()
-        {
-            return toPose3D(robotPose_FieldSpace);
-        }
-        public Pose3d getRobotPose_TargetSpace()
-        {
-            return toPose3D(robotPose_TargetSpace);
-        }
-        public Pose3d getTargetPose_CameraSpace()
-        {
-            return toPose3D(targetPose_CameraSpace);
-        }
-        public Pose3d getTargetPose_RobotSpace()
-        {
-            return toPose3D(targetPose_RobotSpace);
-        }
-
-        public Pose2d getCameraPose_TargetSpace2D()
-        {
-            return toPose2D(cameraPose_TargetSpace);
-        }
-        public Pose2d getRobotPose_FieldSpace2D()
-        {
-            return toPose2D(robotPose_FieldSpace);
-        }
-        public Pose2d getRobotPose_TargetSpace2D()
-        {
-            return toPose2D(robotPose_TargetSpace);
-        }
-        public Pose2d getTargetPose_CameraSpace2D()
-        {
-            return toPose2D(targetPose_CameraSpace);
-        }
-        public Pose2d getTargetPose_RobotSpace2D()
-        {
-            return toPose2D(targetPose_RobotSpace);
-        }
-
-        @JsonProperty("ta")
-        public double ta;
-
-        @JsonProperty("tx")
-        public double tx;
-        
-        @JsonProperty("ty")
-        public double ty;
-
-        @JsonProperty("txp")
-        public double tx_pixels;
-
-        @JsonProperty("typ")
-        public double ty_pixels;
-
-        @JsonProperty("tx_nocross")
-        public double tx_nocrosshair;
-
-        @JsonProperty("ty_nocross")
-        public double ty_nocrosshair;
-
-        @JsonProperty("ts")
-        public double ts;
-
-        public LimelightTarget_Retro() {
-            cameraPose_TargetSpace = new double[6];
-            robotPose_FieldSpace = new double[6];
-            robotPose_TargetSpace = new double[6];
-            targetPose_CameraSpace = new double[6];
-            targetPose_RobotSpace = new double[6];
-        }
-
-    }
-
-    /**
-     * Represents an AprilTag/Fiducial Target Result extracted from JSON Output
-     */
-    public static class LimelightTarget_Fiducial {
-
-        @JsonProperty("fID")
-        public double fiducialID;
-
-        @JsonProperty("fam")
-        public String fiducialFamily;
-
-        @JsonProperty("t6c_ts")
-        private double[] cameraPose_TargetSpace;
-
-        @JsonProperty("t6r_fs")
-        private double[] robotPose_FieldSpace;
-
-        @JsonProperty("t6r_ts")
-        private double[] robotPose_TargetSpace;
-
-        @JsonProperty("t6t_cs")
-        private double[] targetPose_CameraSpace;
-
-        @JsonProperty("t6t_rs")
-        private double[] targetPose_RobotSpace;
-
-        public Pose3d getCameraPose_TargetSpace()
-        {
-            return toPose3D(cameraPose_TargetSpace);
-        }
-        public Pose3d getRobotPose_FieldSpace()
-        {
-            return toPose3D(robotPose_FieldSpace);
-        }
-        public Pose3d getRobotPose_TargetSpace()
-        {
-            return toPose3D(robotPose_TargetSpace);
-        }
-        public Pose3d getTargetPose_CameraSpace()
-        {
-            return toPose3D(targetPose_CameraSpace);
-        }
-        public Pose3d getTargetPose_RobotSpace()
-        {
-            return toPose3D(targetPose_RobotSpace);
-        }
-
-        public Pose2d getCameraPose_TargetSpace2D()
-        {
-            return toPose2D(cameraPose_TargetSpace);
-        }
-        public Pose2d getRobotPose_FieldSpace2D()
-        {
-            return toPose2D(robotPose_FieldSpace);
-        }
-        public Pose2d getRobotPose_TargetSpace2D()
-        {
-            return toPose2D(robotPose_TargetSpace);
-        }
-        public Pose2d getTargetPose_CameraSpace2D()
-        {
-            return toPose2D(targetPose_CameraSpace);
-        }
-        public Pose2d getTargetPose_RobotSpace2D()
-        {
-            return toPose2D(targetPose_RobotSpace);
-        }
-        
-        @JsonProperty("ta")
-        public double ta;
-
-        @JsonProperty("tx")
-        public double tx;
-
-        @JsonProperty("ty")
-        public double ty;
-
-        @JsonProperty("txp")
-        public double tx_pixels;
-
-        @JsonProperty("typ")
-        public double ty_pixels;
-
-        @JsonProperty("tx_nocross")
-        public double tx_nocrosshair;
-
-        @JsonProperty("ty_nocross")
-        public double ty_nocrosshair;
-
-        @JsonProperty("ts")
-        public double ts;
-        
-        public LimelightTarget_Fiducial() {
-            cameraPose_TargetSpace = new double[6];
-            robotPose_FieldSpace = new double[6];
-            robotPose_TargetSpace = new double[6];
-            targetPose_CameraSpace = new double[6];
-            targetPose_RobotSpace = new double[6];
-        }
-    }
-
-    /**
-     * Represents a Barcode Target Result extracted from JSON Output
-     */
-    public static class LimelightTarget_Barcode {
-
-        /**
-         * Barcode family type (e.g. "QR", "DataMatrix", etc.)
-         */
-        @JsonProperty("fam")
-        public String family;
-
-        /**
-         * Gets the decoded data content of the barcode
-         */
-        @JsonProperty("data") 
-        public String data;
-
-        @JsonProperty("txp")
-        public double tx_pixels;
-
-        @JsonProperty("typ")
-        public double ty_pixels;
-
-        @JsonProperty("tx")
-        public double tx;
-
-        @JsonProperty("ty")
-        public double ty;
-
-        @JsonProperty("tx_nocross")
-        public double tx_nocrosshair;
-
-        @JsonProperty("ty_nocross")
-        public double ty_nocrosshair;
-
-        @JsonProperty("ta")
-        public double ta;
-
-        @JsonProperty("pts")
-        public double[][] corners;
-
-        public LimelightTarget_Barcode() {
-        }
-
-        public String getFamily() {
-            return family;
-        }
-    }
-
-    /**
-     * Represents a Neural Classifier Pipeline Result extracted from JSON Output
-     */
-    public static class LimelightTarget_Classifier {
-
-        @JsonProperty("class")
-        public String className;
-
-        @JsonProperty("classID")
-        public double classID;
-
-        @JsonProperty("conf")
-        public double confidence;
-
-        @JsonProperty("zone")
-        public double zone;
-
-        @JsonProperty("tx")
-        public double tx;
-
-        @JsonProperty("txp")
-        public double tx_pixels;
-
-        @JsonProperty("ty")
-        public double ty;
-
-        @JsonProperty("typ")
-        public double ty_pixels;
-
-        public  LimelightTarget_Classifier() {
-        }
-    }
-
-    /**
-     * Represents a Neural Detector Pipeline Result extracted from JSON Output
-     */
-    public static class LimelightTarget_Detector {
-
-        @JsonProperty("class")
-        public String className;
-
-        @JsonProperty("classID")
-        public double classID;
-
-        @JsonProperty("conf")
-        public double confidence;
-
-        @JsonProperty("ta")
-        public double ta;
-
-        @JsonProperty("tx")
-        public double tx;
-
-        @JsonProperty("ty")
-        public double ty;
-
-        @JsonProperty("txp")
-        public double tx_pixels;
-
-        @JsonProperty("typ")
-        public double ty_pixels;
-
-        @JsonProperty("tx_nocross")
-        public double tx_nocrosshair;
-
-        @JsonProperty("ty_nocross")
-        public double ty_nocrosshair;
-
-        public LimelightTarget_Detector() {
-        }
-    }
-
-    /**
-     * Represents hardware statistics from the Limelight.
-     */
-    public static class HardwareReport {
-        @JsonProperty("cid")
-        public String cameraId;
-
-        @JsonProperty("cpu")
-        public double cpuUsage;
-
-        @JsonProperty("dfree")
-        public double diskFree;
-
-        @JsonProperty("dtot")
-        public double diskTotal;
-
-        @JsonProperty("ram")
-        public double ramUsage;
-
-        @JsonProperty("temp")
-        public double temperature;
-
-        public HardwareReport() {
-        }
-    }
-
-    /**
-     * Represents IMU data from the JSON results.
-     */
-    public static class IMUResults {
-        @JsonProperty("data")
-        public double[] data;
-
-        @JsonProperty("quat")
-        public double[] quaternion;
-
-        @JsonProperty("yaw")
-        public double yaw;
-
-        // Parsed from data array
-        public double robotYaw;
-        public double roll;
-        public double pitch;
-        public double rawYaw;
-        public double gyroZ;
-        public double gyroX;
-        public double gyroY;
-        public double accelZ;
-        public double accelX;
-        public double accelY;
-
-        public IMUResults() {
-            data = new double[0];
-            quaternion = new double[4];
-        }
-
-        public void parseDataArray() {
-            if (data != null && data.length >= 10) {
-                robotYaw = data[0];
-                roll = data[1];
-                pitch = data[2];
-                rawYaw = data[3];
-                gyroZ = data[4];
-                gyroX = data[5];
-                gyroY = data[6];
-                accelZ = data[7];
-                accelX = data[8];
-                accelY = data[9];
-            }
-        }
-    }
-
-    /**
-     * Represents capture rewind buffer statistics.
-     */
-    public static class RewindStats {
-        @JsonProperty("bufferUsage")
-        public double bufferUsage;
-
-        @JsonProperty("enabled")
-        public int enabled;
-
-        @JsonProperty("flushing")
-        public int flushing;
-
-        @JsonProperty("frameCount")
-        public int frameCount;
-
-        @JsonProperty("latpen")
-        public int latencyPenalty;
-
-        @JsonProperty("storedSeconds")
-        public double storedSeconds;
-
-        public RewindStats() {
-        }
-    }
-
-    /**
-     * Limelight Results object, parsed from a Limelight's JSON results output.
-     */
-    public static class LimelightResults {
-        
-        public String error;
-        
-        @JsonProperty("pID")
-        public double pipelineID;
-
-        @JsonProperty("tl")
-        public double latency_pipeline;
-
-        @JsonProperty("cl")
-        public double latency_capture;
-
-        public double latency_jsonParse;
-
-        @JsonProperty("ts")
-        public double timestamp_LIMELIGHT_publish;
-
-        @JsonProperty("ts_rio")
-        public double timestamp_RIOFPGA_capture;
-
-        @JsonProperty("ts_nt")
-        public long timestamp_nt;
-
-        @JsonProperty("ts_sys")
-        public long timestamp_sys;
-
-        @JsonProperty("ts_us")
-        public long timestamp_us;
-
-        @JsonProperty("v")
-        @JsonFormat(shape = Shape.NUMBER)
-        public boolean valid;
-
-        @JsonProperty("pTYPE")
-        public String pipelineType;
-
-        @JsonProperty("tx")
-        public double tx;
-
-        @JsonProperty("ty")
-        public double ty;
-
-        @JsonProperty("txnc")
-        public double tx_nocrosshair;
-
-        @JsonProperty("tync")
-        public double ty_nocrosshair;
-
-        @JsonProperty("ta")
-        public double ta;
-
-        @JsonProperty("botpose")
-        public double[] botpose;
-
-        @JsonProperty("botpose_wpired")
-        public double[] botpose_wpired;
-
-        @JsonProperty("botpose_wpiblue")
-        public double[] botpose_wpiblue;
-
-        @JsonProperty("botpose_tagcount")
-        public double botpose_tagcount;
-       
-        @JsonProperty("botpose_span")
-        public double botpose_span;
-       
-        @JsonProperty("botpose_avgdist")
-        public double botpose_avgdist;
-       
-        @JsonProperty("botpose_avgarea")
-        public double botpose_avgarea;
-
-        @JsonProperty("botpose_orb")
-        public double[] botpose_orb;
-
-        @JsonProperty("botpose_orb_wpiblue")
-        public double[] botpose_orb_wpiblue;
-
-        @JsonProperty("botpose_orb_wpired")
-        public double[] botpose_orb_wpired;
-
-        @JsonProperty("t6c_rs")
-        public double[] camerapose_robotspace;
-
-        @JsonProperty("hw")
-        public HardwareReport hardware;
-
-        @JsonProperty("imu")
-        public IMUResults imuResults;
-
-        @JsonProperty("rewind")
-        public RewindStats rewindStats;
-
-        @JsonProperty("PythonOut")
-        public double[] pythonOutput;
-
-        public Pose3d getBotPose3d() {
-            return toPose3D(botpose);
-        }
-    
-        public Pose3d getBotPose3d_wpiRed() {
-            return toPose3D(botpose_wpired);
-        }
-    
-        public Pose3d getBotPose3d_wpiBlue() {
-            return toPose3D(botpose_wpiblue);
-        }
-
-        public Pose2d getBotPose2d() {
-            return toPose2D(botpose);
-        }
-    
-        public Pose2d getBotPose2d_wpiRed() {
-            return toPose2D(botpose_wpired);
-        }
-    
-        public Pose2d getBotPose2d_wpiBlue() {
-            return toPose2D(botpose_wpiblue);
-        }
-
-        @JsonProperty("Retro")
-        public LimelightTarget_Retro[] targets_Retro;
-
-        @JsonProperty("Fiducial")
-        public LimelightTarget_Fiducial[] targets_Fiducials;
-
-        @JsonProperty("Classifier")
-        public LimelightTarget_Classifier[] targets_Classifier;
-
-        @JsonProperty("Detector")
-        public LimelightTarget_Detector[] targets_Detector;
-
-        @JsonProperty("Barcode")
-        public LimelightTarget_Barcode[] targets_Barcode;
-
-        public LimelightResults() {
-            botpose = new double[6];
-            botpose_wpired = new double[6];
-            botpose_wpiblue = new double[6];
-            botpose_orb = new double[6];
-            botpose_orb_wpiblue = new double[6];
-            botpose_orb_wpired = new double[6];
-            camerapose_robotspace = new double[6];
-            targets_Retro = new LimelightTarget_Retro[0];
-            targets_Fiducials = new LimelightTarget_Fiducial[0];
-            targets_Classifier = new LimelightTarget_Classifier[0];
-            targets_Detector = new LimelightTarget_Detector[0];
-            targets_Barcode = new LimelightTarget_Barcode[0];
-            pythonOutput = new double[0];
-            pipelineType = "";
-        }
-
-
-    }
-
-    /**
-     * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output.
-     */
-    public static class RawFiducial {
-        public int id = 0;
-        public double txnc = 0;
-        public double tync = 0;
-        public double ta = 0;
-        public double distToCamera = 0;
-        public double distToRobot = 0;
-        public double ambiguity = 0;
-
-
-        public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) {
-            this.id = id;
-            this.txnc = txnc;
-            this.tync = tync;
-            this.ta = ta;
-            this.distToCamera = distToCamera;
-            this.distToRobot = distToRobot;
-            this.ambiguity = ambiguity;
-        }
-
-        @Override
-        public boolean equals(Object obj) {
-            if (this == obj) return true;
-            if (obj == null || getClass() != obj.getClass()) return false;
-            RawFiducial other = (RawFiducial) obj;
-            return id == other.id &&
-                Double.compare(txnc, other.txnc) == 0 &&
-                Double.compare(tync, other.tync) == 0 &&
-                Double.compare(ta, other.ta) == 0 &&
-                Double.compare(distToCamera, other.distToCamera) == 0 &&
-                Double.compare(distToRobot, other.distToRobot) == 0 &&
-                Double.compare(ambiguity, other.ambiguity) == 0;
-        }
-
-    }
-
-    /**
-     * Represents a Limelight Raw Target/Contour result from Limelight's NetworkTables output.
-     */
-    public static class RawTarget {
-        public double txnc = 0;
-        public double tync = 0;
-        public double ta = 0;
-
-        public RawTarget(double txnc, double tync, double ta) {
-            this.txnc = txnc;
-            this.tync = tync;
-            this.ta = ta;
-        }
-
-        @Override
-        public boolean equals(Object obj) {
-            if (this == obj) return true;
-            if (obj == null || getClass() != obj.getClass()) return false;
-            RawTarget other = (RawTarget) obj;
-            return Double.compare(txnc, other.txnc) == 0 &&
-                Double.compare(tync, other.tync) == 0 &&
-                Double.compare(ta, other.ta) == 0;
-        }
-    }
-
-    /**
-     * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output.
-     */
-    public static class RawDetection {
-        public int classId = 0;
-        public double txnc = 0;
-        public double tync = 0;
-        public double ta = 0;
-        public double corner0_X = 0;
-        public double corner0_Y = 0;
-        public double corner1_X = 0;
-        public double corner1_Y = 0;
-        public double corner2_X = 0;
-        public double corner2_Y = 0;
-        public double corner3_X = 0;
-        public double corner3_Y = 0;
-
-
-        public RawDetection(int classId, double txnc, double tync, double ta, 
-            double corner0_X, double corner0_Y, 
-            double corner1_X, double corner1_Y, 
-            double corner2_X, double corner2_Y, 
-            double corner3_X, double corner3_Y ) {
-            this.classId = classId;
-            this.txnc = txnc;
-            this.tync = tync;
-            this.ta = ta;
-            this.corner0_X = corner0_X;
-            this.corner0_Y = corner0_Y;
-            this.corner1_X = corner1_X;
-            this.corner1_Y = corner1_Y;
-            this.corner2_X = corner2_X;
-            this.corner2_Y = corner2_Y;
-            this.corner3_X = corner3_X;
-            this.corner3_Y = corner3_Y;
-        }
-    }
-    
-    /**
-     * Represents a 3D Pose Estimate.
-     */
-    public static class PoseEstimate {
-        public Pose2d pose;
-        public double timestampSeconds;
-        public double latency;
-        public int tagCount;
-        public double tagSpan;
-        public double avgTagDist;
-        public double avgTagArea;
-
-        public RawFiducial[] rawFiducials; 
-        public boolean isMegaTag2;
-
-        /**
-         * Instantiates a PoseEstimate object with default values
-         */
-        public PoseEstimate() {
-            this.pose = new Pose2d();
-            this.timestampSeconds = 0;
-            this.latency = 0;
-            this.tagCount = 0;
-            this.tagSpan = 0;
-            this.avgTagDist = 0;
-            this.avgTagArea = 0;
-            this.rawFiducials = new RawFiducial[]{};
-            this.isMegaTag2 = false;
-        }
-
-        public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, 
-            int tagCount, double tagSpan, double avgTagDist, 
-            double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) {
-
-            this.pose = pose;
-            this.timestampSeconds = timestampSeconds;
-            this.latency = latency;
-            this.tagCount = tagCount;
-            this.tagSpan = tagSpan;
-            this.avgTagDist = avgTagDist;
-            this.avgTagArea = avgTagArea;
-            this.rawFiducials = rawFiducials;
-            this.isMegaTag2 = isMegaTag2;
-        }
-
-        @Override
-        public boolean equals(Object obj) {
-            if (this == obj) return true;
-            if (obj == null || getClass() != obj.getClass()) return false;
-            PoseEstimate that = (PoseEstimate) obj;
-            // We don't compare the timestampSeconds as it isn't relevant for equality and makes
-            // unit testing harder
-            return Double.compare(that.latency, latency) == 0
-                && tagCount == that.tagCount
-                && Double.compare(that.tagSpan, tagSpan) == 0
-                && Double.compare(that.avgTagDist, avgTagDist) == 0
-                && Double.compare(that.avgTagArea, avgTagArea) == 0
-                && pose.equals(that.pose)
-                && Arrays.equals(rawFiducials, that.rawFiducials);
-        }
-
-    }
-
-    /**
-     * Encapsulates the state of an internal Limelight IMU.
-     */
-    public static class IMUData {
-        public double robotYaw = 0.0;
-        public double Roll = 0.0;
-        public double Pitch = 0.0;
-        public double Yaw = 0.0;
-        public double gyroX = 0.0;
-        public double gyroY = 0.0;
-        public double gyroZ = 0.0;
-        public double accelX = 0.0;
-        public double accelY = 0.0;
-        public double accelZ = 0.0;
-
-        public IMUData() {}
-
-        public IMUData(double[] imuData) {
-            if (imuData != null && imuData.length >= 10) {
-                this.robotYaw = imuData[0];
-                this.Roll = imuData[1];
-                this.Pitch = imuData[2];
-                this.Yaw = imuData[3];
-                this.gyroX = imuData[4];
-                this.gyroY = imuData[5];
-                this.gyroZ = imuData[6];
-                this.accelX = imuData[7];
-                this.accelY = imuData[8];
-                this.accelZ = imuData[9];
-            }
-        }
-    }
-
-
-    private static ObjectMapper mapper;
-
-    /**
-     * Print JSON Parse time to the console in milliseconds
-     */
-    static boolean profileJSON = false;
-
-    static final String sanitizeName(String name) {
-        if ("".equals(name) || name == null) {
-            return "limelight";
-        }
-        return name;
-    }
-
-    /**
-     * Takes a 6-length array of pose data and converts it to a Pose3d object.
-     * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees.
-     * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
-     * @return Pose3d object representing the pose, or empty Pose3d if invalid data
-     */
-    public static Pose3d toPose3D(double[] inData){
-        if(inData.length < 6)
-        {
-            //System.err.println("Bad LL 3D Pose Data!");
-            return new Pose3d();
-        }
-        return new Pose3d(
-            new Translation3d(inData[0], inData[1], inData[2]),
-            new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]),
-                    Units.degreesToRadians(inData[5])));
-    }
-
-    /**
-     * Takes a 6-length array of pose data and converts it to a Pose2d object.
-     * Uses only x, y, and yaw components, ignoring z, roll, and pitch.
-     * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees.
-     * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
-     * @return Pose2d object representing the pose, or empty Pose2d if invalid data
-     */
-    public static Pose2d toPose2D(double[] inData){
-        if(inData.length < 6)
-        {
-            //System.err.println("Bad LL 2D Pose Data!");
-            return new Pose2d();
-        }
-        Translation2d tran2d = new Translation2d(inData[0], inData[1]);
-        Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5]));
-        return new Pose2d(tran2d, r2d);
-    }
-
-    /**
-     * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
-     * Translation components are in meters, rotation components are in degrees.
-     * 
-     * @param pose The Pose3d object to convert
-     * @return A 6-element array containing [x, y, z, roll, pitch, yaw]
-     */
-    public static double[] pose3dToArray(Pose3d pose) {
-        double[] result = new double[6];
-        result[0] = pose.getTranslation().getX();
-        result[1] = pose.getTranslation().getY();
-        result[2] = pose.getTranslation().getZ();
-        result[3] = Units.radiansToDegrees(pose.getRotation().getX());
-        result[4] = Units.radiansToDegrees(pose.getRotation().getY());
-        result[5] = Units.radiansToDegrees(pose.getRotation().getZ());
-        return result;
-    }
-
-    /**
-     * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
-     * Translation components are in meters, rotation components are in degrees.
-     * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw.
-     * 
-     * @param pose The Pose2d object to convert
-     * @return A 6-element array containing [x, y, 0, 0, 0, yaw]
-     */
-    public static double[] pose2dToArray(Pose2d pose) {
-        double[] result = new double[6];
-        result[0] = pose.getTranslation().getX();
-        result[1] = pose.getTranslation().getY();
-        result[2] = 0;
-        result[3] = Units.radiansToDegrees(0);
-        result[4] = Units.radiansToDegrees(0);
-        result[5] = Units.radiansToDegrees(pose.getRotation().getRadians());
-        return result;
-    }
-
-    private static double extractArrayEntry(double[] inData, int position){
-        if(inData.length < position+1)
-        {
-            return 0;
-        }
-        return inData[position];
-    }
-
-    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) {
-        DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName);
-        
-        TimestampedDoubleArray tsValue = poseEntry.getAtomic();
-        double[] poseArray = tsValue.value;
-        long timestamp = tsValue.timestamp;
-        
-        if (poseArray.length == 0) {
-            // Handle the case where no data is available
-            return new PoseEstimate();
-        }
-    
-        var pose = toPose2D(poseArray);
-        double latency = extractArrayEntry(poseArray, 6);
-        int tagCount = (int)extractArrayEntry(poseArray, 7);
-        double tagSpan = extractArrayEntry(poseArray, 8);
-        double tagDist = extractArrayEntry(poseArray, 9);
-        double tagArea = extractArrayEntry(poseArray, 10);
-        
-        // Convert server timestamp from microseconds to seconds and adjust for latency
-        double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0);
-    
-        int valsPerFiducial = 7;
-        int expectedTotalVals = 11 + valsPerFiducial * tagCount;
-        RawFiducial[] rawFiducials;
-
-        if (poseArray.length != expectedTotalVals) {
-            // Array size mismatch - return empty array instead of null-filled array
-            rawFiducials = new RawFiducial[0];
-        } else {
-            rawFiducials = new RawFiducial[tagCount];
-            for(int i = 0; i < tagCount; i++) {
-                int baseIndex = 11 + (i * valsPerFiducial);
-                int id = (int)poseArray[baseIndex];
-                double txnc = poseArray[baseIndex + 1];
-                double tync = poseArray[baseIndex + 2];
-                double ta = poseArray[baseIndex + 3];
-                double distToCamera = poseArray[baseIndex + 4];
-                double distToRobot = poseArray[baseIndex + 5];
-                double ambiguity = poseArray[baseIndex + 6];
-                rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
-            }
-        }
-    
-        return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2);
-    }
-
-    /**
-     * Gets the latest raw fiducial/AprilTag detection results from NetworkTables.
-     * 
-     * @param limelightName Name/identifier of the Limelight
-     * @return Array of RawFiducial objects containing detection details
-     */
-    public static RawFiducial[] getRawFiducials(String limelightName) {
-        var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials");
-        var rawFiducialArray = entry.getDoubleArray(new double[0]);
-        int valsPerEntry = 7;
-        if (rawFiducialArray.length % valsPerEntry != 0) {
-            return new RawFiducial[0];
-        }
-    
-        int numFiducials = rawFiducialArray.length / valsPerEntry;
-        RawFiducial[] rawFiducials = new RawFiducial[numFiducials];
-    
-        for (int i = 0; i < numFiducials; i++) {
-            int baseIndex = i * valsPerEntry;
-            int id = (int) extractArrayEntry(rawFiducialArray, baseIndex);
-            double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1);
-            double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2);
-            double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3);
-            double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4);
-            double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5);
-            double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6);
-            
-            rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
-        }
-    
-        return rawFiducials;
-    }
-
-    /**
-     * Gets the latest raw neural detector results from NetworkTables
-     *
-     * @param limelightName Name/identifier of the Limelight
-     * @return Array of RawDetection objects containing detection details
-     */
-    public static RawDetection[] getRawDetections(String limelightName) {
-        var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections");
-        var rawDetectionArray = entry.getDoubleArray(new double[0]);
-        int valsPerEntry = 12;
-        if (rawDetectionArray.length % valsPerEntry != 0) {
-            return new RawDetection[0];
-        }
-    
-        int numDetections = rawDetectionArray.length / valsPerEntry;
-        RawDetection[] rawDetections = new RawDetection[numDetections];
-    
-        for (int i = 0; i < numDetections; i++) {
-            int baseIndex = i * valsPerEntry; // Starting index for this detection's data
-            int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex);
-            double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1);
-            double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2);
-            double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3);
-            double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4);
-            double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5);
-            double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6);
-            double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7);
-            double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8);
-            double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9);
-            double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10);
-            double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11);
-            
-            rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y);
-        }
-    
-        return rawDetections;
-    }
-
-    /**
-     * Gets the raw target contours from NetworkTables.
-     * Returns ungrouped contours in normalized screen space (-1 to 1).
-     *
-     * @param limelightName Name/identifier of the Limelight
-     * @return Array of RawTarget objects containing up to 3 contours
-     */
-    public static RawTarget[] getRawTargets(String limelightName) {
-        var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawtargets");
-        var rawTargetArray = entry.getDoubleArray(new double[0]);
-        int valsPerEntry = 3;
-        if (rawTargetArray.length % valsPerEntry != 0) {
-            return new RawTarget[0];
-        }
-
-        int numTargets = rawTargetArray.length / valsPerEntry;
-        RawTarget[] rawTargets = new RawTarget[numTargets];
-
-        for (int i = 0; i < numTargets; i++) {
-            int baseIndex = i * valsPerEntry;
-            double txnc = extractArrayEntry(rawTargetArray, baseIndex);
-            double tync = extractArrayEntry(rawTargetArray, baseIndex + 1);
-            double ta = extractArrayEntry(rawTargetArray, baseIndex + 2);
-
-            rawTargets[i] = new RawTarget(txnc, tync, ta);
-        }
-
-        return rawTargets;
-    }
-
-    /**
-     * Gets the corner coordinates of detected targets from NetworkTables.
-     * Requires "send contours" to be enabled in the Limelight Output tab.
-     *
-     * @param limelightName Name/identifier of the Limelight
-     * @return Array of doubles containing corner coordinates [x0, y0, x1, y1, ...]
-     */
-    public static double[] getCornerCoordinates(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "tcornxy");
-    }
-
-    /**
-     * Prints detailed information about a PoseEstimate to standard output.
-     * Includes timestamp, latency, tag count, tag span, average tag distance,
-     * average tag area, and detailed information about each detected fiducial.
-     *
-     * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available."
-     */
-    public static void printPoseEstimate(PoseEstimate pose) {
-        if (pose == null) {
-            System.out.println("No PoseEstimate available.");
-            return;
-        }
-    
-        System.out.printf("Pose Estimate Information:%n");
-        System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds);
-        System.out.printf("Latency: %.3f ms%n", pose.latency);
-        System.out.printf("Tag Count: %d%n", pose.tagCount);
-        System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
-        System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
-        System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
-        System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2);
-        System.out.println();
-    
-        if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
-            System.out.println("No RawFiducials data available.");
-            return;
-        }
-    
-        System.out.println("Raw Fiducials Details:");
-        for (int i = 0; i < pose.rawFiducials.length; i++) {
-            RawFiducial fiducial = pose.rawFiducials[i];
-            System.out.printf(" Fiducial #%d:%n", i + 1);
-            System.out.printf("  ID: %d%n", fiducial.id);
-            System.out.printf("  TXNC: %.2f%n", fiducial.txnc);
-            System.out.printf("  TYNC: %.2f%n", fiducial.tync);
-            System.out.printf("  TA: %.2f%n", fiducial.ta);
-            System.out.printf("  Distance to Camera: %.2f meters%n", fiducial.distToCamera);
-            System.out.printf("  Distance to Robot: %.2f meters%n", fiducial.distToRobot);
-            System.out.printf("  Ambiguity: %.2f%n", fiducial.ambiguity);
-            System.out.println();
-        }
-    }
-
-    public static Boolean validPoseEstimate(PoseEstimate pose) {
-        return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
-    }
-
-    public static NetworkTable getLimelightNTTable(String tableName) {
-        return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
-    }
-
-    public static void Flush() {
-        NetworkTableInstance.getDefault().flush();
-    }
-
-    public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) {
-        return getLimelightNTTable(tableName).getEntry(entryName);
-    }
-
-    public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) {
-        String key = tableName + "/" + entryName;
-        return doubleArrayEntries.computeIfAbsent(key, k -> {
-            NetworkTable table = getLimelightNTTable(tableName);
-            return table.getDoubleArrayTopic(entryName).getEntry(new double[0]);
-        });
-    }
-    
-    public static double getLimelightNTDouble(String tableName, String entryName) {
-        return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0);
-    }
-
-    public static void setLimelightNTDouble(String tableName, String entryName, double val) {
-        getLimelightNTTableEntry(tableName, entryName).setDouble(val);
-    }
-
-    public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) {
-        getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val);
-    }
-
-    public static double[] getLimelightNTDoubleArray(String tableName, String entryName) {
-        return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]);
-    }
-
-
-    public static String getLimelightNTString(String tableName, String entryName) {
-        return getLimelightNTTableEntry(tableName, entryName).getString("");
-    }
-
-    public static String[] getLimelightNTStringArray(String tableName, String entryName) {
-        return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]);
-    }
-
-
-    /////
-
-    /**
-     * Does the Limelight have a valid target?
-     * @param limelightName Name of the Limelight camera ("" for default)
-     * @return True if a valid target is present, false otherwise
-     */
-    public static boolean getTV(String limelightName) {
-        return 1.0 == getLimelightNTDouble(limelightName, "tv");
-    }
-
-    /**
-     * Gets the horizontal offset from the crosshair to the target in degrees.
-     * @param limelightName Name of the Limelight camera ("" for default)
-     * @return Horizontal offset angle in degrees
-     */
-    public static double getTX(String limelightName) {
-        return getLimelightNTDouble(limelightName, "tx");
-    }
-
-    /**
-     * Gets the vertical offset from the crosshair to the target in degrees.
-     * @param limelightName Name of the Limelight camera ("" for default)
-     * @return Vertical offset angle in degrees
-     */
-    public static double getTY(String limelightName) {
-        return getLimelightNTDouble(limelightName, "ty");
-    }
-
-    /**
-     * Gets the horizontal offset from the principal pixel/point to the target in degrees.  This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.
-     * @param limelightName Name of the Limelight camera ("" for default)
-     * @return Horizontal offset angle in degrees
-     */
-    public static double getTXNC(String limelightName) {
-        return getLimelightNTDouble(limelightName, "txnc");
-    }
-
-    /**
-     * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.
-     * @param limelightName Name of the Limelight camera ("" for default)
-     * @return Vertical offset angle in degrees
-     */
-    public static double getTYNC(String limelightName) {
-        return getLimelightNTDouble(limelightName, "tync");
-    }
-
-    /**
-     * Gets the target area as a percentage of the image (0-100%).
-     * @param limelightName Name of the Limelight camera ("" for default) 
-     * @return Target area percentage (0-100)
-     */
-    public static double getTA(String limelightName) {
-        return getLimelightNTDouble(limelightName, "ta");
-    }
-
-    /**
-     * T2D is an array that contains several targeting metrcis
-     * @param limelightName Name of the Limelight camera
-     * @return Array containing  [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, 
-     * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees]
-     */
-    public static double[] getT2DArray(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "t2d");
-    }
-    
-    /**
-     * Gets the number of targets currently detected.
-     * @param limelightName Name of the Limelight camera
-     * @return Number of detected targets
-     */
-    public static int getTargetCount(String limelightName) {
-      double[] t2d = getT2DArray(limelightName);
-      if(t2d.length == 17)
-      {
-        return (int)t2d[1];
-      }
-      return 0;
-    }
-
-    /**
-     * Gets the classifier class index from the currently running neural classifier pipeline
-     * @param limelightName Name of the Limelight camera
-     * @return Class index from classifier pipeline
-     */
-    public static int getClassifierClassIndex (String limelightName) {
-    double[] t2d = getT2DArray(limelightName);
-      if(t2d.length == 17)
-      {
-        return (int)t2d[11];
-      }
-      return 0;
-    }
-
-    /**
-     * Gets the detector class index from the primary result of the currently running neural detector pipeline.
-     * @param limelightName Name of the Limelight camera
-     * @return Class index from detector pipeline
-     */
-    public static int getDetectorClassIndex (String limelightName) {
-     double[] t2d = getT2DArray(limelightName);
-      if(t2d.length == 17)
-      {
-        return (int)t2d[10];
-      }
-      return 0;
-    }
-
-    /**
-     * Gets the current neural classifier result class name.
-     * @param limelightName Name of the Limelight camera
-     * @return Class name string from classifier pipeline
-     */
-    public static String getClassifierClass (String limelightName) {
-        return getLimelightNTString(limelightName, "tcclass");
-    }
-
-    /**
-     * Gets the primary neural detector result class name.
-     * @param limelightName Name of the Limelight camera
-     * @return Class name string from detector pipeline
-     */
-    public static String getDetectorClass (String limelightName) {
-        return getLimelightNTString(limelightName, "tdclass");
-    }
-
-    /**
-     * Gets the pipeline's processing latency contribution.
-     * @param limelightName Name of the Limelight camera
-     * @return Pipeline latency in milliseconds
-     */
-    public static double getLatency_Pipeline(String limelightName) {
-        return getLimelightNTDouble(limelightName, "tl");
-    }
-
-    /**
-     * Gets the capture latency.
-     * @param limelightName Name of the Limelight camera
-     * @return Capture latency in milliseconds
-     */
-    public static double getLatency_Capture(String limelightName) {
-        return getLimelightNTDouble(limelightName, "cl");
-    }
-
-    /**
-     * Gets the active pipeline index.
-     * @param limelightName Name of the Limelight camera
-     * @return Current pipeline index (0-9)
-     */
-    public static double getCurrentPipelineIndex(String limelightName) {
-        return getLimelightNTDouble(limelightName, "getpipe");
-    }
-
-    /**
-     * Gets the current pipeline type.
-     * @param limelightName Name of the Limelight camera
-     * @return Pipeline type string (e.g. "retro", "apriltag", etc)
-     */
-    public static String getCurrentPipelineType(String limelightName) {
-        return getLimelightNTString(limelightName, "getpipetype");
-    }
-
-    /**
-     * Gets the full JSON results dump.
-     * @param limelightName Name of the Limelight camera
-     * @return JSON string containing all current results
-     */
-    public static String getJSONDump(String limelightName) {
-        return getLimelightNTString(limelightName, "json");
-    }
-
-    /**
-     * Switch to getBotPose
-     * 
-     * @param limelightName
-     * @return
-     */
-    @Deprecated
-    public static double[] getBotpose(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "botpose");
-    }
-
-    /**
-     * Switch to getBotPose_wpiRed
-     * 
-     * @param limelightName
-     * @return
-     */
-    @Deprecated
-    public static double[] getBotpose_wpiRed(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
-    }
-
-    /**
-     * Switch to getBotPose_wpiBlue
-     * 
-     * @param limelightName
-     * @return
-     */
-    @Deprecated
-    public static double[] getBotpose_wpiBlue(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
-    }
-
-    public static double[] getBotPose(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "botpose");
-    }
-
-    public static double[] getBotPose_wpiRed(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
-    }
-
-    public static double[] getBotPose_wpiBlue(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
-    }
-
-    public static double[] getBotPose_TargetSpace(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
-    }
-
-    public static double[] getCameraPose_TargetSpace(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
-    }
-
-    public static double[] getTargetPose_CameraSpace(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
-    }
-
-    public static double[] getTargetPose_RobotSpace(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
-    }
-
-    /**
-     * Gets the average color under the crosshair region as a 3-element array.
-     * @param limelightName Name of the Limelight camera
-     * @return Array containing [Blue, Green, Red] color values (BGR order)
-     */
-    public static double[] getTargetColor(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "tc");
-    }
-
-    public static double getFiducialID(String limelightName) {
-        return getLimelightNTDouble(limelightName, "tid");
-    }
-
-    /**
-     * Gets the Limelight heartbeat value. Increments once per frame, allowing you to detect if the Limelight is connected and alive.
-     * @param limelightName Name of the Limelight camera
-     * @return Heartbeat value that increments each frame
-     */
-    public static double getHeartbeat(String limelightName) {
-        return getLimelightNTDouble(limelightName, "hb");
-    }
-
-    public static String getNeuralClassID(String limelightName) {
-        return getLimelightNTString(limelightName, "tclass");
-    }
-
-    public static String[] getRawBarcodeData(String limelightName) {
-        return getLimelightNTStringArray(limelightName, "rawbarcodes");
-    }
-
-    /////
-    /////
-
-    public static Pose3d getBotPose3d(String limelightName) {
-        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose");
-        return toPose3D(poseArray);
-    }
-
-    /**
-     * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System.
-     * @param limelightName Name/identifier of the Limelight
-     * @return Pose3d object representing the robot's position and orientation in Red Alliance field space
-     */
-    public static Pose3d getBotPose3d_wpiRed(String limelightName) {
-        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired");
-        return toPose3D(poseArray);
-    }
-
-    /**
-     * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System.
-     * @param limelightName Name/identifier of the Limelight
-     * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space
-     */
-    public static Pose3d getBotPose3d_wpiBlue(String limelightName) {
-        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
-        return toPose3D(poseArray);
-    }
-
-    /**
-     * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system.
-     * @param limelightName Name/identifier of the Limelight
-     * @return Pose3d object representing the robot's position and orientation relative to the target
-     */
-    public static Pose3d getBotPose3d_TargetSpace(String limelightName) {
-        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
-        return toPose3D(poseArray);
-    }
-
-    /**
-     * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system.
-     * @param limelightName Name/identifier of the Limelight
-     * @return Pose3d object representing the camera's position and orientation relative to the target
-     */
-    public static Pose3d getCameraPose3d_TargetSpace(String limelightName) {
-        double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
-        return toPose3D(poseArray);
-    }
-
-    /**
-     * Gets the target's 3D pose with respect to the camera's coordinate system.
-     * @param limelightName Name/identifier of the Limelight
-     * @return Pose3d object representing the target's position and orientation relative to the camera
-     */
-    public static Pose3d getTargetPose3d_CameraSpace(String limelightName) {
-        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
-        return toPose3D(poseArray);
-    }
-
-    /**
-     * Gets the target's 3D pose with respect to the robot's coordinate system.
-     * @param limelightName Name/identifier of the Limelight
-     * @return Pose3d object representing the target's position and orientation relative to the robot
-     */
-    public static Pose3d getTargetPose3d_RobotSpace(String limelightName) {
-        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
-        return toPose3D(poseArray);
-    }
-
-    /**
-     * Gets the camera's 3D pose with respect to the robot's coordinate system.
-     * @param limelightName Name/identifier of the Limelight
-     * @return Pose3d object representing the camera's position and orientation relative to the robot
-     */
-    public static Pose3d getCameraPose3d_RobotSpace(String limelightName) {
-        double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace");
-        return toPose3D(poseArray);
-    }
-
-    /**
-     * Gets the Pose2d for easy use with Odometry vision pose estimator
-     * (addVisionMeasurement)
-     * 
-     * @param limelightName
-     * @return
-     */
-    public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
-
-        double[] result = getBotPose_wpiBlue(limelightName);
-        return toPose2D(result);
-    }
-
-    /**
-     * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
-     * 
-     * @param limelightName
-     * @return
-     */
-    public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_wpiblue", false);
-    }
-
-    /**
-     * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
-     * Make sure you are calling setRobotOrientation() before calling this method.
-     * 
-     * @param limelightName
-     * @return
-     */
-    public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true);
-    }
-
-    /**
-     * Gets the Pose2d for easy use with Odometry vision pose estimator
-     * (addVisionMeasurement)
-     * 
-     * @param limelightName
-     * @return
-     */
-    public static Pose2d getBotPose2d_wpiRed(String limelightName) {
-
-        double[] result = getBotPose_wpiRed(limelightName);
-        return toPose2D(result);
-
-    }
-
-    /**
-     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
-     * alliance
-     * @param limelightName
-     * @return
-     */
-    public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_wpired", false);
-    }
-
-    /**
-     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
-     * alliance
-     * @param limelightName
-     * @return
-     */
-    public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true);
-    }
-
-    /**
-     * Gets the Pose2d for easy use with Odometry vision pose estimator
-     * (addVisionMeasurement)
-     * 
-     * @param limelightName
-     * @return
-     */
-    public static Pose2d getBotPose2d(String limelightName) {
-
-        double[] result = getBotPose(limelightName);
-        return toPose2D(result);
-
-    }
-   
-    /**
-     * Gets the current IMU data from NetworkTables.
-     * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ].
-     * Returns all zeros if data is invalid or unavailable.
-     * 
-     * @param limelightName Name/identifier of the Limelight
-     * @return IMUData object containing all current IMU data
-     */
-    public static IMUData getIMUData(String limelightName) {
-        double[] imuData = getLimelightNTDoubleArray(limelightName, "imu");
-        if (imuData == null || imuData.length < 10) {
-            return new IMUData();  // Returns object with all zeros
-        }
-        return new IMUData(imuData);
-    }
-
-    /////
-    /////
-
-    public static void setPipelineIndex(String limelightName, int pipelineIndex) {
-        setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
-    }
-
-    
-    public static void setPriorityTagID(String limelightName, int ID) {
-        setLimelightNTDouble(limelightName, "priorityid", ID);
-    }
-
-    /**
-     * Sets LED mode to be controlled by the current pipeline.
-     * @param limelightName Name of the Limelight camera
-     */
-    public static void setLEDMode_PipelineControl(String limelightName) {
-        setLimelightNTDouble(limelightName, "ledMode", 0);
-    }
-
-    public static void setLEDMode_ForceOff(String limelightName) {
-        setLimelightNTDouble(limelightName, "ledMode", 1);
-    }
-
-    public static void setLEDMode_ForceBlink(String limelightName) {
-        setLimelightNTDouble(limelightName, "ledMode", 2);
-    }
-
-    public static void setLEDMode_ForceOn(String limelightName) {
-        setLimelightNTDouble(limelightName, "ledMode", 3);
-    }
-
-    /**
-     * Enables standard side-by-side stream mode.
-     * @param limelightName Name of the Limelight camera
-     */
-    public static void setStreamMode_Standard(String limelightName) {
-        setLimelightNTDouble(limelightName, "stream", 0);
-    }
-
-    /**
-     * Enables Picture-in-Picture mode with secondary stream in the corner.
-     * @param limelightName Name of the Limelight camera
-     */
-    public static void setStreamMode_PiPMain(String limelightName) {
-        setLimelightNTDouble(limelightName, "stream", 1);
-    }
-
-    /**
-     * Enables Picture-in-Picture mode with primary stream in the corner.
-     * @param limelightName Name of the Limelight camera
-     */
-    public static void setStreamMode_PiPSecondary(String limelightName) {
-        setLimelightNTDouble(limelightName, "stream", 2);
-    }
-
-
-    /**
-     * Sets the crop window for the camera. The crop window in the UI must be completely open.
-     * @param limelightName Name of the Limelight camera
-     * @param cropXMin Minimum X value (-1 to 1)
-     * @param cropXMax Maximum X value (-1 to 1)
-     * @param cropYMin Minimum Y value (-1 to 1)
-     * @param cropYMax Maximum Y value (-1 to 1)
-     */
-    public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) {
-        double[] entries = new double[4];
-        entries[0] = cropXMin;
-        entries[1] = cropXMax;
-        entries[2] = cropYMin;
-        entries[3] = cropYMax;
-        setLimelightNTDoubleArray(limelightName, "crop", entries);
-    }
-
-    /**
-     * Sets the keystone modification for the crop window.
-     * @param limelightName Name of the Limelight camera
-     * @param horizontal Horizontal keystone value (-0.95 to 0.95)
-     * @param vertical Vertical keystone value (-0.95 to 0.95)
-     */
-    public static void setKeystone(String limelightName, double horizontal, double vertical) {
-        double[] entries = new double[2];
-        entries[0] = horizontal;
-        entries[1] = vertical;
-        setLimelightNTDoubleArray(limelightName, "keystone_set", entries);
-    }
-
-    /**
-     * Sets 3D offset point for easy 3D targeting.
-     */
-    public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) {
-        double[] entries = new double[3];
-        entries[0] = offsetX;
-        entries[1] = offsetY;
-        entries[2] = offsetZ;
-        setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries);
-    }
-
-    /**
-     * Sets robot orientation values used by MegaTag2 localization algorithm.
-     * 
-     * @param limelightName Name/identifier of the Limelight
-     * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC
-     * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second
-     * @param pitch (Unnecessary) Robot pitch in degrees 
-     * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second
-     * @param roll (Unnecessary) Robot roll in degrees
-     * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second
-     */
-    public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, 
-        double pitch, double pitchRate, 
-        double roll, double rollRate) {
-        SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true);
-    }
-
-    public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, 
-        double pitch, double pitchRate, 
-        double roll, double rollRate) {
-        SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false);
-    }
-
-    private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, 
-        double pitch, double pitchRate, 
-        double roll, double rollRate, boolean flush) {
-
-        double[] entries = new double[6];
-        entries[0] = yaw;
-        entries[1] = yawRate;
-        entries[2] = pitch;
-        entries[3] = pitchRate;
-        entries[4] = roll;
-        entries[5] = rollRate;
-        setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
-        if(flush)
-        {
-            Flush();
-        }
-    }
-   
-    /**
-     * Configures the IMU mode for MegaTag2 Localization
-     * 
-     * @param limelightName Name/identifier of the Limelight
-     * @param mode IMU mode.
-     */
-    public static void SetIMUMode(String limelightName, int mode) {
-        setLimelightNTDouble(limelightName, "imumode_set", mode);
-    }
-
-    /**
-     * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4)
-     * 
-     * @param limelightName Name/identifier of the Limelight
-     * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the assist source more rapidly.
-     */
-    public static void SetIMUAssistAlpha(String limelightName, double alpha) {
-        setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha);
-    }
-
-    
-    /**
-     * Configures the throttle value. Set to 100-200 while disabled to reduce thermal output/temperature.
-     * 
-     * @param limelightName Name/identifier of the Limelight
-     * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping <throttle> frames.
-     */
-    public static void SetThrottle(String limelightName, int throttle) {
-        setLimelightNTDouble(limelightName, "throttle_set", throttle);
-    }
-
-    /**
-     * Overrides the valid AprilTag IDs that will be used for localization.
-     * Tags not in this list will be ignored for robot pose estimation.
-     *
-     * @param limelightName Name/identifier of the Limelight
-     * @param validIDs Array of valid AprilTag IDs to track
-     */
-    public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) {
-        double[] validIDsDouble = new double[validIDs.length];
-        for (int i = 0; i < validIDs.length; i++) {
-            validIDsDouble[i] = validIDs[i];
-        }        
-        setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble);
-    }
-
-    /**
-     * Sets the downscaling factor for AprilTag detection.
-     * Increasing downscale can improve performance at the cost of potentially reduced detection range.
-     * 
-     * @param limelightName Name/identifier of the Limelight
-     * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control.
-     */
-    public static void SetFiducialDownscalingOverride(String limelightName, float downscale) 
-    {
-        int d = 0; // pipeline
-        if (downscale == 1.0)
-        {
-            d = 1;
-        }
-        if (downscale == 1.5)
-        {
-            d = 2;
-        }
-        if (downscale == 2)
-        {
-            d = 3;
-        }
-        if (downscale == 3)
-        {
-            d = 4;
-        }
-        if (downscale == 4)
-        {
-            d = 5;
-        }
-        setLimelightNTDouble(limelightName, "fiducial_downscale_set", d);
-    }
-    
-    /**
-     * Sets the camera pose relative to the robot.
-     * @param limelightName Name of the Limelight camera
-     * @param forward Forward offset in meters
-     * @param side Side offset in meters
-     * @param up Up offset in meters
-     * @param roll Roll angle in degrees
-     * @param pitch Pitch angle in degrees
-     * @param yaw Yaw angle in degrees
-     */
-    public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) {
-        double[] entries = new double[6];
-        entries[0] = forward;
-        entries[1] = side;
-        entries[2] = up;
-        entries[3] = roll;
-        entries[4] = pitch;
-        entries[5] = yaw;
-        setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries);
-    }
-
-    /////
-    /////
-
-    public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) {
-        setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData);
-    }
-
-    public static double[] getPythonScriptData(String limelightName) {
-        return getLimelightNTDoubleArray(limelightName, "llpython");
-    }
-
-    /////
-    /////
-
-    /**
-     * Triggers a snapshot capture via NetworkTables by incrementing the snapshot counter.
-     * Rate-limited to once per 10 frames on the Limelight.
-     * @param limelightName Name of the Limelight camera
-     */
-    public static void triggerSnapshot(String limelightName) {
-        double current = getLimelightNTDouble(limelightName, "snapshot");
-        setLimelightNTDouble(limelightName, "snapshot", current + 1);
-    }
-
-    /**
-     * Enables or pauses the rewind buffer recording.
-     * @param limelightName Name of the Limelight camera
-     * @param enabled True to enable recording, false to pause
-     */
-    public static void setRewindEnabled(String limelightName, boolean enabled) {
-        setLimelightNTDouble(limelightName, "rewind_enable_set", enabled ? 1 : 0);
-    }
-
-    /**
-     * Triggers a rewind capture with the specified duration.
-     * Maximum duration is 165 seconds. Rate-limited on the Limelight.
-     * @param limelightName Name of the Limelight camera
-     * @param durationSeconds Duration of rewind capture in seconds (max 165)
-     */
-    public static void triggerRewindCapture(String limelightName, double durationSeconds) {
-        double[] currentArray = getLimelightNTDoubleArray(limelightName, "capture_rewind");
-        double counter = (currentArray.length > 0) ? currentArray[0] : 0;
-        double[] entries = new double[2];
-        entries[0] = counter + 1;
-        entries[1] = Math.min(durationSeconds, 165);
-        setLimelightNTDoubleArray(limelightName, "capture_rewind", entries);
-    }
-
-    /**
-     * Gets the latest JSON results output and returns a LimelightResults object.
-     * @param limelightName Name of the Limelight camera
-     * @return LimelightResults object containing all current target data
-     */
-    public static LimelightResults getLatestResults(String limelightName) {
-
-        long start = System.nanoTime();
-        LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults();
-        if (mapper == null) {
-            mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false);
-        }
-
-        try {
-            String jsonString = getJSONDump(limelightName);
-            if (jsonString == null || jsonString.isEmpty() || jsonString.isBlank()) {
-                results.error = "lljson error: empty json";
-            } else {
-                results = mapper.readValue(jsonString, LimelightResults.class);
-                if (results.imuResults != null) {
-                    results.imuResults.parseDataArray();
-                }
-            }
-        } catch (JsonProcessingException e) {
-            results.error = "lljson error: " + e.getMessage();
-        }
-
-        long end = System.nanoTime();
-        double millis = (end - start) * .000001;
-        results.latency_jsonParse = millis;
-        if (profileJSON) {
-            System.out.printf("lljson: %.2f\r\n", millis);
-        }
-
-        return results;
-    }
-
-    /**
-     * Sets up port forwarding for a Limelight 3A/3G connected via USB.
-     * This allows access to the Limelight web interface and video stream
-     * when connected to the robot over USB.
-     *
-     * For usbIndex 0: ports 5800-5809 forward to 172.29.0.1
-     * For usbIndex 1: ports 5810-5819 forward to 172.29.1.1
-     * etc.
-     *
-     * Call this method once during robot initialization.
-     * To access the interface of the camera with usbIndex0, you would go to roboRIO-(teamnum)-FRC.local:5801. Port 5811 for usb index 1
-     *
-     * @param usbIndex The USB index of the Limelight (0, 1, 2, etc.)
-     */
-    public static void setupPortForwardingUSB(int usbIndex) {
-        String ip = "172.29." + usbIndex + ".1";
-        int basePort = 5800 + (usbIndex * 10);
-
-        for (int i = 0; i < 10; i++) {
-            PortForwarder.add(basePort + i, ip, 5800 + i);
-        }
-    }
-}
\ 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 27c4874..aa34047 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -28,6 +28,7 @@ import frc.robot.Constants.IntakeConstants;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.TargetingSubsystems;
 
 /**
@@ -44,10 +45,10 @@ public class Robot extends TimedRobot {
     private RobotContainer m_robotContainer;
     private LEDSubsystem m_LedSubsystem;
     private ClimberSubsystem m_ClimberSubsystem;
+    private ShooterSubsystem m_ShooterSubsystem;
 
     private Timer disabledTimer;
     private static NetworkTable table;
-    private static GenericEntry distanceFromLimelight;
 
     private static StructPublisher<Pose2d> advantageScopePublisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose hahaha", Pose2d.struct).publish();
 
@@ -72,6 +73,7 @@ public class Robot extends TimedRobot {
         m_LedSubsystem = new LEDSubsystem();
         m_robotContainer = new RobotContainer();
         m_ClimberSubsystem = new ClimberSubsystem();
+        m_ShooterSubsystem = new ShooterSubsystem();
         // Create a timer to disable motor brake a few seconds after disable. This will
         // let the robot stop
         // immediately when disabled, but then also let it be pushed more
@@ -145,7 +147,7 @@ public class Robot extends TimedRobot {
        // IntakeSubsystem.resetIntakeRotationEncoder();
         m_robotContainer.setMotorBrake(true);
         m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-        m_ClimberSubsystem.lowerRobotCommand();
+        //ShooterSubsystem.setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM);
 
         // Print the selected autonomous command upon autonomous init
         System.out.println("Auto selected: " + m_autonomousCommand);
@@ -168,7 +170,8 @@ public class Robot extends TimedRobot {
 
     @Override
     public void teleopInit() {
-        m_ClimberSubsystem.lowerRobotCommand();
+        ClimberSubsystem.lowerRobot();
+        //ShooterSubsystem.setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM);
         // This makes sure that the autonomous stops running when
         // teleop starts running. If you want the autonomous to
         // continue until interrupted by another command, remove
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 842e492..a02106d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
@@ -43,6 +44,7 @@ import javax.lang.model.util.ElementScanner14;
 import frc.robot.subsystems.TargetingSubsystems;
 import swervelib.SwerveInputStream;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.swervedrive.Vision;
 
@@ -59,8 +61,8 @@ public class RobotContainer {
         // Replace with CommandPS4Controller or CommandJoystick if needed
         final CommandXboxController driverXbox = new CommandXboxController(0);
         final CommandXboxController operatorXbox = new CommandXboxController(1);
-                private final static CommandJoystick topButtons = new CommandJoystick(2);
-
+        private final static CommandJoystick topButtons = new CommandJoystick(2);
+        final CommandJoystick bottomButtons = new CommandJoystick(3);
 
         // The robot's subsystems and commands are defined here...
         private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
@@ -71,8 +73,7 @@ public class RobotContainer {
         private final SendableChooser<Command> autoChooser;
 
         private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
-        private final TargetingSubsystems m_TargetingSubsystems = new
-         TargetingSubsystems();
+        private final TargetingSubsystems m_TargetingSubsystems = new TargetingSubsystems();
         private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
         private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
         /**
@@ -142,21 +143,37 @@ public class RobotContainer {
 
                 // Create the NamedCommands that will be used in PathPlanner
                 NamedCommands.registerCommand("test", Commands.print("I EXIST"));
+                NamedCommands.registerCommand("Start_Intake_Wheels", m_IntakeSubsystem.startIntakeMotorCommand(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST));
                 NamedCommands.registerCommand("Shoot_Fuel_Command",
-                                m_ShooterSubsystem.setShooterMotorsRPMAutoCommand()
-                                                .andThen(m_IntakeSubsystem.startIntakeMotorCommand()));
-                NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployIntakeCommand());
-                NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand());
+                                m_ShooterSubsystem.setShooterMotorsRPMAutoCommand());
+                NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployIntakeCommand()
+                                .andThen(m_IntakeSubsystem.startIntakeMotorCommand(
+                                                Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_SLOW)));
+                NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand()
+                                .andThen(m_IntakeSubsystem.deployIntakeCommand()));
                 NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand());
-                NamedCommands.registerCommand("Assist_Shooter", m_IntakeSubsystem.assistFuelIntakeCommand());
+                NamedCommands.registerCommand("Assist_Shooter",
+                                m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly().withTimeout(5));
                 NamedCommands.registerCommand("Lift_Robot", m_ClimberSubsystem.liftRobotCommand());
                 NamedCommands.registerCommand("Kill_All", killAllCommand());
-                NamedCommands.registerCommand("Auto_Aim_To_Hub", m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
-                NamedCommands.registerCommand("PathPlan_To_Climb_Right_Offsetted", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
-                NamedCommands.registerCommand("PathPlan_To_Climb_Left_Offsetted", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
-                NamedCommands.registerCommand("PathPlan_Into_Climb_Right", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE));
-                NamedCommands.registerCommand("PathPlan_Into_Climb_Left", drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE));
-
+                NamedCommands.registerCommand("Auto_Aim_To_Hub",
+                                m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
+                
+                
+                /*NamedCommands.registerCommand("PathPlan_To_Climb_Right_Offsetted",
+                                drivebase.driveToClimbPoseOffsetted(
+                                                Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED,
+                                                Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
+                NamedCommands.registerCommand("PathPlan_To_Climb_Left_Offsetted",
+                                drivebase.driveToClimbPoseOffsetted(
+                                                Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED,
+                                                Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
+                NamedCommands.registerCommand("PathPlan_Into_Climb_Right",
+                                drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE,
+                                                Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE));
+                NamedCommands.registerCommand("PathPlan_Into_Climb_Left",
+                                drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE,
+                                                Constants.TargetingConstants.RED_LEFT_CLIMB_POSE));*/
 
                 // Have the autoChooser pull in all PathPlanner autos as options
                 autoChooser = AutoBuilder.buildAutoChooser();
@@ -173,132 +190,173 @@ public class RobotContainer {
 
         }
 
-    /**
-     * Use this method to define your trigger->command mappings. Triggers can be
-     * created via the
-     * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
-     * an arbitrary predicate, or via the
-     * named factories in
-     * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
-     * for
-     * {@link CommandXboxController
-     * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
-     * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
-     * Flight joysticks}.
-     */
-    private void configureBindings() {
-        Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
-        Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
-        Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
-        Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
-                driveDirectAngle);
-        Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
-        Command driveFieldOrientedAnglularVelocityKeyboard = drivebase
-                .driveFieldOriented(driveAngularVelocityKeyboard);
-        Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
-                driveDirectAngleKeyboard);
+        /**
+         * Use this method to define your trigger->command mappings. Triggers can be
+         * created via the
+         * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
+         * an arbitrary predicate, or via the
+         * named factories in
+         * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
+         * for
+         * {@link CommandXboxController
+         * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
+         * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
+         * Flight joysticks}.
+         */
+        private void configureBindings() {
+                Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
+                Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
+                Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
+                Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
+                                driveDirectAngle);
+                Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
+                Command driveFieldOrientedAnglularVelocityKeyboard = drivebase
+                                .driveFieldOriented(driveAngularVelocityKeyboard);
+                Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
+                                driveDirectAngleKeyboard);
 
-        driverXbox.leftTrigger().whileTrue(m_IntakeSubsystem.startIntakeMotorCommand())
-                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
-        driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand().andThen(m_ShooterSubsystem.reverseIndexerAndRampMotorRPMCommand()))
-                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand().andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand()));
-        // command for
-        // full shooting system including linear actuators
-        driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
-        driverXbox.rightBumper().whileTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
-        //driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly());
+                driverXbox.start().onTrue(m_IntakeSubsystem.stopIntakeMotorCommand());
+                driverXbox.back().onTrue((Commands.runOnce(drivebase::zeroGyro)));
 
+                driverXbox.leftTrigger().onTrue(m_IntakeSubsystem
+                                .startIntakeMotorCommand(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST));
+                driverXbox.rightBumper()
+                                .whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand()
+                                                .andThen(m_ShooterSubsystem.reverseIndexerAndRampMotorRPMCommand())
+                                                .andThen(m_ShooterSubsystem.reverseShooterCommand()))
+                                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()
+                                                .andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand())
+                                                .andThen(m_ShooterSubsystem.setShooterMotorsRPMCommand(
+                                                                Constants.ShooterConstants.IDLE_SHOOTER_RPM)));
+                // command for
+                // full shooting system including linear actuators
+                //driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand().andThen(new WaitCommand(1.5)));
+                                //.andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
+                driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.setShooterMotorsRPMCommand(-3400));
+                driverXbox.leftBumper().onTrue(m_IntakeSubsystem
+                                .startIntakeMotorCommand(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_SLOW));
+                // driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE,
+                // Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly());
 
-        driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand());
-        driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand());
-        //driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY));
-        //driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT));
-        //driverXbox.povLeft().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE));
+                driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand());
+                driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand());
+                // driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY));
+                // driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT));
+                // driverXbox.povLeft().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE));
 
-        driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand());
-        driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand());
-        //driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
+                driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand());
+                driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand());
+                // driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase,
+                // driverXbox));
 
-        // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
-        driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand());
-                //.andThen(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)));
-        // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
-        // () -> -driverXbox.getLeftX()));
+                // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
+                driverXbox.x().onTrue(setIdleShooterRPMCommand().andThen(m_IntakeSubsystem.deployIntakeCommand()));
+                // .andThen(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)));
+                // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
+                // () -> -driverXbox.getLeftX()));
 
-        // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
+                // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
 
-        operatorXbox.x().whileTrue(m_ShooterSubsystem.testLeftShooterCommand())
-                .onFalse(m_ShooterSubsystem.stopLeftShooterCommand());
-        operatorXbox.y().whileTrue(m_ShooterSubsystem.testCenterShooterCommand())
-                .onFalse(m_ShooterSubsystem.stopCenterShooterCommand());
-        operatorXbox.b().whileTrue(m_ShooterSubsystem.testRightShooterCommand())
-                .onFalse(m_ShooterSubsystem.stopRightShooterCommand());
-        operatorXbox.a().whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand())
-                .onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
+                
+                  operatorXbox.x().whileTrue(m_ShooterSubsystem.testLeftShooterCommand())
+                  .onFalse(m_ShooterSubsystem.stopLeftShooterCommand());
+                  operatorXbox.y().whileTrue(m_ShooterSubsystem.testCenterShooterCommand())
+                  .onFalse(m_ShooterSubsystem.stopCenterShooterCommand());
+                  operatorXbox.b().whileTrue(m_ShooterSubsystem.testRightShooterCommand())
+                  .onFalse(m_ShooterSubsystem.stopRightShooterCommand());
+                 operatorXbox.a().whileTrue(m_ShooterSubsystem.
+                  setIndexerAndRampMotorRPMCommand())
+                  .onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
+                 
 
-        topButtons.axisGreaterThan(1, 0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -5)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
-        topButtons.axisGreaterThan(1, -0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
-        topButtons.button(3).onTrue(killAllCommand());
-        topButtons.button(6).whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
-        topButtons.button(1).onTrue(drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
-        topButtons.button(2).onTrue(drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
+                topButtons.axisGreaterThan(1, 0.3)
+                                .toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(
+                                                Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -5))
+                                .toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
+                topButtons.axisGreaterThan(1, -0.3)
+                                .toggleOnTrue(m_IntakeSubsystem
+                                                .rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED))
+                                .toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
+                topButtons.axisGreaterThan(0, 0.5).onTrue(m_ClimberSubsystem.setClimberSpeedCommand(0.4))
+                                .onFalse(m_ClimberSubsystem.setClimberSpeedCommand(0));
+                topButtons.axisLessThan(0, -0.8).onTrue(m_ClimberSubsystem.setClimberSpeedCommand(-0.4))
+                                .onFalse(m_ClimberSubsystem.setClimberSpeedCommand(0));
+                topButtons.button(3).onTrue(killAllCommand());
+                topButtons.button(6).whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
+                topButtons.button(1)
+                                .onTrue(drivebase.driveToClimbPoseOffsetted(
+                                                Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED,
+                                                Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
+                topButtons.button(2)
+                                .onTrue(drivebase.driveToClimbPoseOffsetted(
+                                                Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED,
+                                                Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
+                topButtons.button(4).whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand())
+                                .onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
 
-        //topButtons.button(1).onTrue(drivebase.driveToPose(Constants.))
+                bottomButtons.button(9)
+                                .onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly().withTimeout(1.5));
+                bottomButtons.button(3).whileTrue(m_IntakeSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+                bottomButtons.button(7).whileTrue(m_IntakeSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+                bottomButtons.button(4).whileTrue(m_IntakeSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+                bottomButtons.button(8).whileTrue(m_IntakeSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
 
-        if (RobotBase.isSimulation()) {
-            drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
-        } else {
-            drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
-        }
+                // topButtons.button(1).onTrue(drivebase.driveToPose(Constants.))
 
-        if (Robot.isSimulation()) {
-            Pose2d target = new Pose2d(new Translation2d(1, 4),
-                    Rotation2d.fromDegrees(90));
-            // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
-            driveDirectAngleKeyboard.driveToPose(() -> target,
-                    new ProfiledPIDController(5,
-                            0,
-                            0,
-                            new Constraints(5, 2)),
-                    new ProfiledPIDController(5,
-                            0,
-                            0,
-                            new Constraints(Units.degreesToRadians(360),
-                                    Units.degreesToRadians(180))));
-            driverXbox.start()
-                    .onTrue(Commands.runOnce(() -> drivebase
-                            .resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
-            driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
-            driverXbox.button(2)
-                    .whileTrue(Commands.runEnd(
-                            () -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
-                            () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
+                if (RobotBase.isSimulation()) {
+                        drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
+                } else {
+                        drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
+                }
 
-            // driverXbox.b().whileTrue(
-            // drivebase.driveToPose(
-            // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
-            // );
+                if (Robot.isSimulation()) {
+                        Pose2d target = new Pose2d(new Translation2d(1, 4),
+                                        Rotation2d.fromDegrees(90));
+                        // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
+                        driveDirectAngleKeyboard.driveToPose(() -> target,
+                                        new ProfiledPIDController(5,
+                                                        0,
+                                                        0,
+                                                        new Constraints(5, 2)),
+                                        new ProfiledPIDController(5,
+                                                        0,
+                                                        0,
+                                                        new Constraints(Units.degreesToRadians(360),
+                                                                        Units.degreesToRadians(180))));
+                        driverXbox.start()
+                                        .onTrue(Commands.runOnce(() -> drivebase
+                                                        .resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
+                        driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
+                        driverXbox.button(2)
+                                        .whileTrue(Commands.runEnd(
+                                                        () -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
+                                                        () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
+
+                        // driverXbox.b().whileTrue(
+                        // drivebase.driveToPose(
+                        // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
+                        // );
+
+                }
+                if (DriverStation.isTest()) {
+                        drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command
+                                                                                         // above!
+
+                        driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
+                        driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
+                        driverXbox.back().whileTrue(drivebase.centerModulesCommand());
+                        driverXbox.leftBumper().onTrue(Commands.none());
+                        driverXbox.rightBumper().onTrue(Commands.none());
+                } else {
+                        // driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
+                        // driverXbox.start().whileTrue(Commands.none());
+                        // driverXbox.back().whileTrue(Commands.none());
+                        // driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock,
+                        // drivebase).repeatedly());
+                        // driverXbox.rightBumper().onTrue(Commands.none());
+                }
 
         }
-        if (DriverStation.isTest()) {
-            drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command
-                                                                             // above!
-
-            driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
-            driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
-            driverXbox.back().whileTrue(drivebase.centerModulesCommand());
-            driverXbox.leftBumper().onTrue(Commands.none());
-            driverXbox.rightBumper().onTrue(Commands.none());
-        } else {
-            driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
-            driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
-            driverXbox.start().whileTrue(Commands.none());
-            driverXbox.back().whileTrue(Commands.none());
-            driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
-            driverXbox.rightBumper().onTrue(Commands.none());
-        }
-
-    }
 
         /**
          * Use this to pass the autonomous command to the main {@link Robot} class.
@@ -323,8 +381,14 @@ public class RobotContainer {
                 CommandScheduler.getInstance().cancelAll();
         }
 
-        public Command killAllCommand(){
-                return Commands.runOnce(()->killAll());
+        public Command killAllCommand() {
+                return Commands.runOnce(() -> killAll());
+        }
+
+        public Command setIdleShooterRPMCommand() {
+                return Commands.runOnce(() -> m_ShooterSubsystem
+                                .setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM))
+                                .andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
         }
 
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 7a1c863..8dcd0e9 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -34,11 +34,18 @@ public class ClimberSubsystem extends SubsystemBase {
     climberMotorPIDController = climberMotor.getClosedLoopController();
   }
 
+  public void setClimberSpeed(double speed) {
+    climberMotor.set(speed);
+  }
+
+  public Command setClimberSpeedCommand(double speed) {
+    return runOnce(() -> setClimberSpeed(speed));
+  }
   public void liftRobot() {
     climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LIFTED_SETPOINT_VALUE, ControlType.kPosition);
   }
 
-  public void lowerRobot() {
+  public static void lowerRobot() {
     climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LOWERED_SETPOINT_VALUE, ControlType.kPosition);
   }
 
@@ -62,5 +69,6 @@ public class ClimberSubsystem extends SubsystemBase {
   @Override
   public void periodic() {
     SmartDashboard.putNumber("Climber Motor Encoder", climberMotor.getEncoder().getPosition());
+    SmartDashboard.putNumber("Climber motor power", climberMotor.get());
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 2ef8428..dcbd473 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -2,6 +2,8 @@ package frc.robot.subsystems;
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -9,6 +11,7 @@ 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.WaitCommand;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 
 import com.revrobotics.spark.ClosedLoopSlot;
@@ -19,6 +22,13 @@ import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.config.SparkFlexConfig;
+
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.VoltsPerRadianPerSecond;
+
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkBase.ControlType;
 
@@ -30,8 +40,17 @@ public class IntakeSubsystem extends SubsystemBase {
     private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
             MotorType.kBrushless);
 
-    private final TrapezoidProfile.Constraints m_Constraints = new TrapezoidProfile.Constraints(5, 10);
+    private final TrapezoidProfile.Constraints m_Constraints = new TrapezoidProfile.Constraints(0.3, .5);
     private final ProfiledPIDController intakeRotatorProfiledPIDController;
+    private static TrapezoidProfile.State goalState = new TrapezoidProfile.State(
+            Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT, 0);
+
+    SysIdRoutine routine = new SysIdRoutine(new SysIdRoutine.Config(),
+            new SysIdRoutine.Mechanism(intakeRotatorMotor::setVoltage,
+            log -> log.motor("arm").voltage(Volts.of(intakeRotatorMotor.getAppliedOutput() * 12))
+                    .angularPosition(Radians.of(intakeRotatorEncoder.get() * 2 * Math.PI))
+                    .angularVelocity(RadiansPerSecond.of(0)),
+                    this, "armSysId"));
 
     private static SparkClosedLoopController intakeRotatorPIDController;
     public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
@@ -44,71 +63,71 @@ public class IntakeSubsystem extends SubsystemBase {
     private static double encoderValue = intakeRotatorEncoder.get();
 
     public IntakeSubsystem() {
-       intakeRotatorProfiledPIDController = new ProfiledPIDController(
-            Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
-            Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
-            Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D, m_Constraints);
-        intakeRotatorProfiledPIDController.setTolerance(0.05);
-        //intakeRotatorProfiledPIDController.setGoal(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT);
+        intakeRotatorProfiledPIDController = new ProfiledPIDController(
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D,
+                m_Constraints,
+                0.02);
+        intakeRotatorProfiledPIDController.setTolerance(0.02);
+        intakeRotatorProfiledPIDController.setGoal(goalState);
 
         intakeRotatorConfig.closedLoop
-                //Slot 0
+                // Slot 0
                 .p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P)
                 .i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I)
                 .d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D)
 
-                //Slot 1
+                // Slot 1
                 .p(.05, ClosedLoopSlot.kSlot1)
                 .i(0, ClosedLoopSlot.kSlot1)
                 .d(.05, ClosedLoopSlot.kSlot1)
 
-                //Slot 2
-                .p(1,ClosedLoopSlot.kSlot2)
+                // Slot 2
+                .p(.8, ClosedLoopSlot.kSlot2)
                 .i(.001, ClosedLoopSlot.kSlot2)
-                .d(0.1, ClosedLoopSlot.kSlot2);
+                .d(0.3, ClosedLoopSlot.kSlot2);
 
+        intakeRotatorConfig.smartCurrentLimit(80);
         intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
                 com.revrobotics.PersistMode.kNoPersistParameters);
-        intakeRotatorConfig.smartCurrentLimit(40);
+
         intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
-        
 
         intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P,
                 Constants.IntakeConstants.INTAKE_MOTOR_I,
                 Constants.IntakeConstants.INTAKE_MOTOR_D);
+        intakeWheelsMotorConfig.smartCurrentLimit(70);
         intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
                 com.revrobotics.PersistMode.kNoPersistParameters);
-        intakeRotatorMotor.getEncoder().setPosition(-5);
+
+        intakeRotatorMotor.getEncoder().setPosition(-6);
         intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
     }
 
-    /* public void goToPosition(double goalPosition) {
-        intakeRotatorProfiledPIDController.setGoal(goalPosition);
-        //double pidVal = intakeRotatorProfiledPIDController.calculate(encoderValue, goalPosition);  
-    } 
+    public void goToPosition(double goalPosition) {
+        goalState = new TrapezoidProfile.State(goalPosition, 0);
+    }
 
     public Command goToPositionCommand(double goalPosition) {
         return runOnce(() -> goToPosition(goalPosition));
-    } 
-        */
+    }
 
-    public void rotateIntake(double speed)
-    {
+    public void rotateIntake(double speed) {
         intakeRotatorMotor.set(speed);
     }
 
-    public Command rotateIntakeCommand(double speed)
-    {
-        return runOnce(()->intakeRotatorMotor.set(speed));
+    public Command rotateIntakeCommand(double speed) {
+        return runOnce(() -> intakeRotatorMotor.set(speed));
     }
 
-    public void startIntakeMotor() {
-        intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM,
+    public void startIntakeMotor(double speed) {
+        intakeWheelsMotorPIDController.setSetpoint(speed,
                 ControlType.kVelocity);
     }
 
     public void reverseIntakeMotor() {
-        intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1,
+        intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST * -1,
                 ControlType.kVelocity);
     }
 
@@ -116,8 +135,8 @@ public class IntakeSubsystem extends SubsystemBase {
         intakeWheelsMotor.set(0);
     }
 
-    public Command startIntakeMotorCommand() {
-        return runOnce(() -> startIntakeMotor());
+    public Command startIntakeMotorCommand(double speed) {
+        return runOnce(() -> startIntakeMotor(speed));
     }
 
     public Command reverseIntakeMotorCommand() {
@@ -128,54 +147,68 @@ public class IntakeSubsystem extends SubsystemBase {
         return runOnce(() -> stopIntakeMotor());
     }
 
-    /* public Command assistFuelIntakeCommand(double deployedPosition, double assistPosition) {
-        return runOnce(() -> goToPositionCommand(assistPosition).andThen(new WaitCommand(1.5))
-                .andThen(goToPositionCommand(deployedPosition)).andThen(new WaitCommand(1.5)));
-    } */
+    /*
+     * public Command assistFuelIntakeCommand(double deployedPosition, double
+     * assistPosition) {
+     * return runOnce(() -> goToPositionCommand(assistPosition).andThen(new
+     * WaitCommand(1.5))
+     * .andThen(goToPositionCommand(deployedPosition)).andThen(new
+     * WaitCommand(1.5)));
+     * }
+     */
 
     public static void resetIntakeRotationEncoder() {
         intakeRotatorMotor.getEncoder().setPosition(-5);
     }
-    
-     public void deployIntake() {
-      intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.
-      INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0);
-      }
-      
-      public Command deployIntakeCommand() {
-      return runOnce(() -> deployIntake());
-      }
-      
-     public void retractIntake() {
-      intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.
-      INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1);
-      }
-     
-      public Command retractIntakeCommand() {
-      return runOnce(() -> retractIntake());
-      }
-      
-      public void assistFuelIntake() {
-     intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.
-      INTAKE_MIDDLE_ENCODER_VALUE,
-      ControlType.kPosition, ClosedLoopSlot.kSlot2);
-      }
-      
-      public Command assistFuelIntakeCommand() {
-      return runOnce(() -> assistFuelIntake()).andThen(new
-      WaitCommand(1)).andThen(deployIntakeCommand())
-      .andThen(new WaitCommand(1));
-      }
-    
+
+    public void deployIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
+                ControlType.kPosition, ClosedLoopSlot.kSlot0);
+    }
+
+    public void deployIntakeAssist() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
+                ControlType.kPosition, ClosedLoopSlot.kSlot2);
+    }
+
+    public Command deployIntakeCommand() {
+        return runOnce(() -> deployIntake());
+    }
+
+    public void retractIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE,
+                ControlType.kPosition, ClosedLoopSlot.kSlot1);
+    }
+
+    public Command retractIntakeCommand() {
+        return runOnce(() -> retractIntake());
+    }
+
+    public void assistFuelIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
+                ControlType.kPosition, ClosedLoopSlot.kSlot2);
+    }
+
+    public Command assistFuelIntakeCommand() {
+        return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(.5)).andThen(deployIntakeCommand())
+                .andThen(new WaitCommand(.5));
+    }
+
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return routine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return routine.dynamic(direction);
+    }
 
     @Override
     public void periodic() {
-        //encoderValue = intakeRotatorEncoder.get();
-        //intakeRotatorMotor.setVoltage(intakeRotatorProfiledPIDController.calculate(encoderValue) * -12);
-        SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
-        SmartDashboard.putNumber("Intake Rotator Encoder Value", intakeRotatorEncoder.get());
-        SmartDashboard.putNumber("Voltage Deploy", intakeRotatorProfiledPIDController.calculate(encoderValue, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY));
-        SmartDashboard.putNumber("Voltage Retract", intakeRotatorProfiledPIDController.calculate(encoderValue, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT));
-
+        encoderValue = intakeRotatorEncoder.get();
+        //intakeRotatorMotor.setVoltage(intakeRotatorProfiledPIDController.calculate(encoderValue, goalState));
+        SmartDashboard.putNumber("Intake Rotator Motor Encoder", intakeRotatorMotor.getEncoder().getPosition());
+        SmartDashboard.putNumber("Intake Rotator Throughbore Encoder Value", intakeRotatorEncoder.get());
+        SmartDashboard.putNumber("Voltage Deploy", intakeRotatorProfiledPIDController.calculate(encoderValue,
+                goalState));
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java
index 5657af5..4bc63b5 100644
--- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java
@@ -31,13 +31,13 @@ public class LEDSubsystem extends SubsystemBase {
   LEDPattern hubActiveBlinkPattern = hubActiveColor.blink(Second.of(0.5));
 
   LEDPattern hubInactiveColor = LEDPattern.solid(Color.kRed);
-  LEDPattern hubInactiveBlinkPattern = hubInactiveColor.blink(Second.of(0.5));
+  LEDPattern hubInactiveBlinkPattern = hubInactiveColor.blink(Second.of(0.1));
 
   LEDPattern allianceShiftColor = LEDPattern.gradient(LEDPattern.GradientType.kContinuous, Color.kTeal, Color.kMagenta);
   LEDPattern allianceShiftPattern = allianceShiftColor.scrollAtRelativeSpeed(Percent.per(Second).of(100));
 
   LEDPattern transitionColor = LEDPattern.solid(Color.kYellow);
-  LEDPattern transitionBlinkPattern = transitionColor.blink(Second.of(0.5));
+  LEDPattern transitionBlinkPattern = transitionColor.blink(Second.of(0.2));
 
   LEDPattern endGameColor = LEDPattern.solid(Color.kOrangeRed);
   LEDPattern endGameBlinkPattern = endGameColor.blink(Second.of(0.2));
@@ -69,22 +69,47 @@ public class LEDSubsystem extends SubsystemBase {
       allianceShiftPattern.applyTo(m_Buffer);
     }
 
-    if (matchTime <= 140 && matchTime > 130 && !DriverStation.isAutonomous()) // transition
+    else
     {
-      transitionBlinkPattern.applyTo(m_Left);
-    }
+      if (matchTime <= 140 && matchTime > 132) // transition
+      {
+        transitionBlinkPattern.applyTo(m_Left);
+      }
 
-    if (matchTime <= 130 && matchTime > 30 && !DriverStation.isAutonomous()) // shifts
-    {
-      allianceShiftPattern.applyTo(m_Left);
-    }
+      if(matchTime <= 132 && matchTime > 130)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
 
-    if (matchTime <= 30 && !DriverStation.isAutonomous()) // endgame
-    {
-      endGameBlinkPattern.applyTo(m_Left);
-    }
+      if(matchTime <= 107 && matchTime > 105 && isHubActive == false)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
 
-    allianceShiftPattern.applyTo(m_Buffer);
+      if(matchTime <= 82 && matchTime > 80 && isHubActive == false)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
+
+      if(matchTime <= 57 && matchTime > 55 && isHubActive == false)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
+
+      if(matchTime <= 32 && matchTime > 30 && isHubActive == false)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
+
+      if (matchTime <= 30) // endgame
+      {
+        endGameBlinkPattern.applyTo(m_Left);
+      }
+
+
+      allianceShiftPattern.applyTo(m_Buffer);
+    }
+    
   }
 
   public void setLEDHubActive() {
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 5810914..06743a2 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -26,7 +26,6 @@ import com.revrobotics.spark.config.SparkFlexConfig;
 import com.revrobotics.spark.config.SparkBaseConfig;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
-import frc.robot.LimelightHelpers;
 
 public class ShooterSubsystem extends SubsystemBase {
     private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
@@ -57,6 +56,7 @@ public class ShooterSubsystem extends SubsystemBase {
         centerShooterMotorConfig
                 .voltageCompensation(12.0)
                 .closedLoop
+                .outputRange(-1, 0)
                 .p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
                 .i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
                 .d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
@@ -64,7 +64,8 @@ public class ShooterSubsystem extends SubsystemBase {
                 .feedForward
                 .kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
                 .kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
-        centerShooterMotorConfig.smartCurrentLimit(80);
+        centerShooterMotorConfig.smartCurrentLimit(45);
+
         centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
                 com.revrobotics.PersistMode.kNoPersistParameters);
         centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController();
@@ -72,6 +73,7 @@ public class ShooterSubsystem extends SubsystemBase {
         leftShooterMotorConfig              
                 .voltageCompensation(12.0)
                 .closedLoop
+                .outputRange(-1, 0)
                 .p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
                 .i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
                 .d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
@@ -79,7 +81,7 @@ public class ShooterSubsystem extends SubsystemBase {
                 .feedForward
                 .kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
                 .kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
-        leftShooterMotorConfig.smartCurrentLimit(80);
+        leftShooterMotorConfig.smartCurrentLimit(45);
         leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
                 com.revrobotics.PersistMode.kNoPersistParameters);
         leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController();
@@ -87,6 +89,7 @@ public class ShooterSubsystem extends SubsystemBase {
         rightShooterMotorConfig
                 .voltageCompensation(12.0)
                 .closedLoop
+                .outputRange(-1, 0)
                 .p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
                 .i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
                 .d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
@@ -94,7 +97,7 @@ public class ShooterSubsystem extends SubsystemBase {
                 .feedForward
                 .kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
                 .kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
-        rightShooterMotorConfig.smartCurrentLimit(80);
+        rightShooterMotorConfig.smartCurrentLimit(45);
         rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
                 com.revrobotics.PersistMode.kNoPersistParameters);
         rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController();
@@ -102,6 +105,7 @@ public class ShooterSubsystem extends SubsystemBase {
         indexerAndRampMotorConfig.closedLoop.pid(Constants.ShooterConstants.INDEXER_MOTOR_P,
                 0,
                 0);
+        indexerAndRampMotorConfig.smartCurrentLimit(50);
         indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
                 com.revrobotics.PersistMode.kNoPersistParameters);
         indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController();
@@ -121,21 +125,25 @@ public class ShooterSubsystem extends SubsystemBase {
         rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
     }
 
-    public void setShooterMotorsRPMAuto(){
-        centerShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity);
-        leftShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity);
-        rightShooterMotorPIDController.setSetpoint(-2700, ControlType.kVelocity);
+    public static void setShooterMotorsRPM(double RPM){
+        centerShooterMotorPIDController.setSetpoint(RPM, ControlType.kVelocity);
+        leftShooterMotorPIDController.setSetpoint(RPM, ControlType.kVelocity);
+        rightShooterMotorPIDController.setSetpoint(RPM, ControlType.kVelocity);
+    }
+
+    public Command setShooterMotorsRPMCommand(double RPM) {
+        return runOnce(() -> setShooterMotorsRPM(RPM));
     }
 
     public Command setShooterMotorsRPMAutoCommand()
     {
-        return runOnce(()-> setShooterMotorsRPMAuto()).andThen(new WaitCommand(1.5))
+        return runOnce(()-> setShooterMotorsRPM(Constants.ShooterConstants.AUTO_SHOOTER_RPM)).andThen(new WaitCommand(1))
       .andThen(() -> setIndexerAndRampMotorRPM());
     }
 
     // test individual motor code
     public void setLeftShooterMotorRPM() {
-        leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
+        leftShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
     }
 
     public Command testLeftShooterCommand() {
@@ -143,7 +151,7 @@ public class ShooterSubsystem extends SubsystemBase {
     }
 
     public void setRightShooterMotorRPM() {
-        rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
+        rightShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
     }
 
     public Command testRightShooterCommand() {
@@ -151,7 +159,7 @@ public class ShooterSubsystem extends SubsystemBase {
     }
 
     public void setCenterShooterMotorRPM() {
-        centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
+        centerShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
     }
 
     public Command testCenterShooterCommand() {
@@ -191,6 +199,7 @@ public class ShooterSubsystem extends SubsystemBase {
                 ControlType.kVelocity);
     }
 
+
     public void reverseIndexerAndRampMotorRPM() {
         indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1,
                 ControlType.kVelocity);
@@ -218,7 +227,7 @@ public class ShooterSubsystem extends SubsystemBase {
 
     
     public Command shootFuelCommand() {
-      return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(1.5))
+      return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(1))
       .andThen(() -> setIndexerAndRampMotorRPM());
       };
 
@@ -244,6 +253,17 @@ public class ShooterSubsystem extends SubsystemBase {
         return runOnce(() -> stopShooters());
     }
 
+    public void reverseShooter()
+    {
+        centerShooterMotor.set(.4);
+        leftShooterMotor.set(0.4);
+        rightShooterMotor.set(0.4);
+    }
+    public Command reverseShooterCommand()
+    {
+        return runOnce(()-> reverseShooter());
+    }
+
     @Override
     public void periodic() {
 
diff --git a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java
index 5dbe141..0602c5a 100644
--- a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java
+++ b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java
@@ -35,7 +35,6 @@ import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import frc.robot.Constants;
-import frc.robot.LimelightHelpers;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -53,7 +52,7 @@ import frc.robot.Constants;
 
 public class TargetingSubsystems extends SubsystemBase {
 
-    PIDController photonAimPIDController = new PIDController(3, 0.01, 0);
+    PIDController photonAimPIDController = new PIDController(5, 0.01, 0);
 
     public static Rotation2d hubThetaPose = new Rotation2d();
     public static Optional<Alliance> alliance = DriverStation.getAlliance();
@@ -138,8 +137,8 @@ public class TargetingSubsystems extends SubsystemBase {
 
             rot = MathUtil.clamp(rot, -3.0, 3.0);
 
-            swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1,
-                    driverXbox.getLeftY() * -1), rot, false);
+            swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
+                    driverXbox.getLeftX() * -1), rot, false);
         }, swerveDrive);
     }
 
@@ -168,12 +167,10 @@ public class TargetingSubsystems extends SubsystemBase {
       public static void updateShooterRPM(Pose2d currentRobotPose) {
       double distance = PhotonUtils.getDistanceToPose(currentRobotPose,
       Constants.TargetingConstants.allianceHubPose);
-      Constants.ShooterConstants.SHOOTER_RPM = 
-      (-293.84123 * Math.pow(distance, 3))
+      Constants.ShooterConstants.SHOOTER_RPM = Math.max((-293.84123 * Math.pow(distance, 3))
       + (1360.01497 * Math.pow(distance, 2))
       - (2391.17127 * distance)
-      - 1249.22704;
-      
+      - 1249.22704, -6000); 
      }
      
 
@@ -185,17 +182,5 @@ public class TargetingSubsystems extends SubsystemBase {
 
         SmartDashboard.putString("Hub Pose", "x: " + Constants.TargetingConstants.allianceHubPose.getMeasureX() + "  y: " + Constants.TargetingConstants.allianceHubPose.getY()
                 + "  angle: " + Constants.TargetingConstants.allianceHubPose.getRotation());
-
-        /*
-         * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
-         * photonVision.getLatestResult().getBestTarget().getYaw());
-         * Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value",
-         * photonVision.getLatestResult().getBestTarget().getPitch());
-         * Shuffleboard.getTab("Vision").add("Limelight TX Value",
-         * LimelightHelpers.getTX("limelight"));
-         * Shuffleboard.getTab("Vision").add("Limelight April Tag ID",
-         * LimelightHelpers.getFiducialID("limelight"));
-         */
-
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
index bbaf78c..fbcb155 100644
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -124,7 +124,7 @@ public class SwerveSubsystem extends SubsystemBase {
             setupPhotonVision();
             // Stop the odometry thread if we are using vision that way we can synchronize
             // updates better.
-            //swerveDrive.stopOdometryThread();
+            swerveDrive.stopOdometryThread();
         }
         setupPathPlanner();
         SmartDashboard.putData("Rebuilt Field", rebuiltField);