diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
index 3519769a..0276c8e2 100644
--- a/.pathplanner/settings.json
+++ b/.pathplanner/settings.json
@@ -1,10 +1,10 @@
 {
-  "robotWidth": 0.89,
-  "robotLength": 0.89,
+  "robotWidth": 0.7493,
+  "robotLength": 0.7493,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 4.0,
+  "defaultMaxVel": 2.0,
   "defaultMaxAccel": 2.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
diff --git a/simulation.properties b/simulation.properties
index 2d763eed..b731d52c 100644
--- a/simulation.properties
+++ b/simulation.properties
@@ -1 +1 @@
-robot.name=STUB
\ No newline at end of file
+robot.name=NEMO
\ No newline at end of file
diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json
index 4a536fc5..7c118ccc 100644
--- a/src/main/deploy/elastic-layout.json
+++ b/src/main/deploy/elastic-layout.json
@@ -1,6 +1,6 @@
 {
   "version": 1.0,
-  "grid_size": 64,
+  "grid_size": 32,
   "tabs": [
     {
       "name": "Teleoperated",
@@ -11,8 +11,8 @@
             "title": "Field",
             "x": 0.0,
             "y": 0.0,
-            "width": 768.0,
-            "height": 384.0,
+            "width": 1024.0,
+            "height": 640.0,
             "type": "Field",
             "properties": {
               "topic": "/SmartDashboard/Field",
@@ -29,8 +29,8 @@
           },
           {
             "title": "Pigeon 2 (v6) [50]",
-            "x": 768.0,
-            "y": 0.0,
+            "x": 1088.0,
+            "y": 128.0,
             "width": 256.0,
             "height": 256.0,
             "type": "Gyro",
@@ -42,8 +42,8 @@
           },
           {
             "title": "BatteryVoltage",
-            "x": 0.0,
-            "y": 384.0,
+            "x": 1024.0,
+            "y": 0.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Voltage View",
@@ -98,6 +98,18 @@
               "robot_color": 4294198070,
               "trajectory_color": 4294967295
             }
+          },
+          {
+            "title": "photonvision_Port_1186_Output_MJPEG_Server",
+            "x": 896.0,
+            "y": 64.0,
+            "width": 576.0,
+            "height": 448.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
+              "period": 0.06
+            }
           }
         ]
       }
@@ -117,18 +129,6 @@
               "label_position": "TOP"
             },
             "children": []
-          },
-          {
-            "title": "Reef Location",
-            "x": 576.0,
-            "y": 256.0,
-            "width": 512.0,
-            "height": 128.0,
-            "type": "List Layout",
-            "properties": {
-              "label_position": "TOP"
-            },
-            "children": []
           }
         ],
         "containers": [
@@ -215,6 +215,289 @@
               "period": 0.06,
               "data_type": "double"
             }
+          },
+          {
+            "title": "Pose choices",
+            "x": 1152.0,
+            "y": 192.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "ComboBox Chooser",
+            "properties": {
+              "topic": "/SmartDashboard/Pose choices",
+              "period": 0.06,
+              "sort_options": false
+            }
+          },
+          {
+            "title": "AAAAAA Reef Position",
+            "x": 1152.0,
+            "y": 384.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/AAAAAA Reef Position",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Chosen Pose Index",
+            "x": 832.0,
+            "y": 448.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Pose Index",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "NEW Driver",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Field",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 768.0,
+            "height": 384.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/SmartDashboard/Field",
+              "period": 0.06,
+              "field_game": "Reefscape",
+              "robot_width": 0.85,
+              "robot_length": 0.85,
+              "show_other_objects": true,
+              "show_trajectories": true,
+              "field_rotation": 0.0,
+              "robot_color": 4294198070,
+              "trajectory_color": 4294967295
+            }
+          },
+          {
+            "title": "Auto Calibrate Elevator",
+            "x": 256.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Elevator[Coral]/Commands/Auto Calibrate Elevator",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Auto Calibrate Coral Arm",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Arm[Coral]/Commands/Auto Calibrate Coral Arm",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Arm Angle (deg)",
+            "x": 0.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Arm[Coral]/PositionDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Elevetor Position (m)",
+            "x": 256.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Elevator[Coral]/PositionMeters",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Alliance Color",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/FMSInfo/IsRedAlliance",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4294901760,
+              "false_color": 4278190335,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Is Field Orient",
+            "x": 768.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/DriveSwerveYAGSL/FieldOrientedDrive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "RIGHT CAM",
+            "x": 1152.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1184_Output_MJPEG_Server",
+              "period": 0.06,
+              "fps": 10,
+              "resolution": [
+                160.0,
+                120.0
+              ]
+            }
+          },
+          {
+            "title": "Distance to Right Tag",
+            "x": 1152.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/front_cam",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "LEFT CAM",
+            "x": 896.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
+              "period": 0.06,
+              "resolution": [
+                160.0,
+                120.0
+              ]
+            }
+          },
+          {
+            "title": "Distance to Left Tag",
+            "x": 896.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/left_cam",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Reef Level",
+            "x": 768.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Driver /Misc/Prepare Selection",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Chosen Reef",
+            "x": 768.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Reef Position",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Auto Calibrate Climber",
+            "x": 512.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Arm[Climber]/Commands/Climber Auto Calibrate ",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Climber Angle (deg)",
+            "x": 512.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Simple[Climber]/PositionDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "USB Camera 0",
+            "x": 896.0,
+            "y": 384.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/USB Camera 0",
+              "period": 0.06
+            }
           }
         ]
       }
@@ -749,8 +1032,7 @@
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server",
-              "period": 0.06,
-              "rotation_turns": 0
+              "period": 0.06
             }
           },
           {
@@ -762,8 +1044,7 @@
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1184_Output_MJPEG_Server",
-              "period": 0.06,
-              "rotation_turns": 0
+              "period": 0.06
             }
           },
           {
@@ -775,8 +1056,7 @@
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1188_Output_MJPEG_Server",
-              "period": 0.06,
-              "rotation_turns": 0
+              "period": 0.06
             }
           },
           {
@@ -788,8 +1068,7 @@
             "type": "Camera Stream",
             "properties": {
               "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
-              "period": 0.06,
-              "rotation_turns": 0
+              "period": 0.06
             }
           }
         ]
@@ -1931,125 +2210,14 @@
     {
       "name": "Drive",
       "grid_layout": {
-        "layouts": [
-          {
-            "title": "rotationPID",
-            "x": 896.0,
-            "y": 256.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "List Layout",
-            "properties": {
-              "label_position": "TOP"
-            },
-            "children": [
-              {
-                "title": "P",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/rotationPID/P",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "I",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/rotationPID/I",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "D",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/rotationPID/D",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              }
-            ]
-          },
-          {
-            "title": "translationPID",
-            "x": 896.0,
-            "y": 0.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "List Layout",
-            "properties": {
-              "label_position": "TOP"
-            },
-            "children": [
-              {
-                "title": "P",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/translationPID/P",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "I",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/translationPID/I",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              },
-              {
-                "title": "D",
-                "x": 0.0,
-                "y": 0.0,
-                "width": 128.0,
-                "height": 128.0,
-                "type": "Text Display",
-                "properties": {
-                  "topic": "/SmartDashboard/Drive[YAGSL]/translationPID/D",
-                  "period": 0.06,
-                  "data_type": "double",
-                  "show_submit_button": true
-                }
-              }
-            ]
-          }
-        ],
-        "containers": [
+        "layouts": [],
+        "containers": [
           {
             "title": "Field",
             "x": 0.0,
             "y": 0.0,
-            "width": 896.0,
-            "height": 512.0,
+            "width": 640.0,
+            "height": 320.0,
             "type": "Field",
             "properties": {
               "topic": "/SmartDashboard/Field",
@@ -2063,6 +2231,349 @@
               "robot_color": 4294198070,
               "trajectory_color": 4294967295
             }
+          },
+          {
+            "title": "Auto Calibrate Elevator",
+            "x": 256.0,
+            "y": 448.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Elevator[Coral]/Commands/Auto Calibrate Elevator",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Auto Calibrate Coral Arm",
+            "x": 0.0,
+            "y": 448.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Command",
+            "properties": {
+              "topic": "/SmartDashboard/Arm[Coral]/Commands/Auto Calibrate Coral Arm",
+              "period": 0.06,
+              "show_type": true
+            }
+          },
+          {
+            "title": "Arm Angle (deg)",
+            "x": 0.0,
+            "y": 576.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Arm[Coral]/PositionDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Elevetor Position (m)",
+            "x": 256.0,
+            "y": 576.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Elevator[Coral]/PositionMeters",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "X (m)",
+            "x": 640.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/PoseX",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Y (m)",
+            "x": 768.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/PoseY",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Rot (deg)",
+            "x": 896.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/PoseRotInDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Flip X (m)",
+            "x": 1152.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/FlippedPoseX",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Flip Y (m)",
+            "x": 1280.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/FlippedPoseY",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Flip Rot (deg)",
+            "x": 1408.0,
+            "y": 576.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/Drive/FlippedPoseRotInDegrees",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "BatteryVoltage",
+            "x": 1152.0,
+            "y": 448.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Voltage View",
+            "properties": {
+              "topic": "/AdvantageKit/SystemStats/BatteryVoltage",
+              "period": 0.06,
+              "data_type": "double",
+              "min_value": 4.0,
+              "max_value": 13.0,
+              "divisions": 5,
+              "inverted": false,
+              "orientation": "horizontal"
+            }
+          },
+          {
+            "title": "Alliance Color",
+            "x": 640.0,
+            "y": 0.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/FMSInfo/IsRedAlliance",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4294901760,
+              "false_color": 4278190335,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Arm[Coral]",
+            "x": 0.0,
+            "y": 320.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Subsystem",
+            "properties": {
+              "topic": "/SmartDashboard/Arm[Coral]",
+              "period": 0.06
+            }
+          },
+          {
+            "title": "Elevator[Coral]",
+            "x": 256.0,
+            "y": 320.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Subsystem",
+            "properties": {
+              "topic": "/SmartDashboard/Elevator[Coral]",
+              "period": 0.06
+            }
+          },
+          {
+            "title": "Is Field Orient",
+            "x": 512.0,
+            "y": 320.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/DriveSwerveYAGSL/FieldOrientedDrive",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "RIGHT CAM",
+            "x": 1088.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1184_Output_MJPEG_Server",
+              "period": 0.06,
+              "fps": 10,
+              "resolution": [
+                160.0,
+                120.0
+              ]
+            }
+          },
+          {
+            "title": "Distance to Right Tag",
+            "x": 1088.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/front_cam",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "LEFT CAM",
+            "x": 832.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server",
+              "period": 0.06,
+              "resolution": [
+                160.0,
+                120.0
+              ]
+            }
+          },
+          {
+            "title": "Distance to Left Tag",
+            "x": 832.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/VisionSubsystem/Distances To Target/left_cam",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Reef Level Selection",
+            "x": 640.0,
+            "y": 256.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Driver /Misc/Prepare Selection",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Chosen Reef Position",
+            "x": 640.0,
+            "y": 128.0,
+            "width": 192.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Reef Position",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Reef Rot (deg)",
+            "x": 896.0,
+            "y": 448.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Reef Rot",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Reef X (m)",
+            "x": 640.0,
+            "y": 448.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Reef X",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Reef Y (m)",
+            "x": 768.0,
+            "y": 448.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Chosen Reef Y",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
           }
         ]
       }
diff --git a/src/main/deploy/pathplanner/autos/Go Straight.auto b/src/main/deploy/pathplanner/autos/Go Straight.auto
deleted file mode 100644
index 5d7449c1..00000000
--- a/src/main/deploy/pathplanner/autos/Go Straight.auto	
+++ /dev/null
@@ -1,50 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Straight"
-          }
-        },
-        {
-          "type": "parallel",
-          "data": {
-            "commands": [
-              {
-                "type": "named",
-                "data": {
-                  "name": "Move Elevator"
-                }
-              },
-              {
-                "type": "named",
-                "data": {
-                  "name": "Move Arm High"
-                }
-              }
-            ]
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Go Straight 1 Meter"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Move Arm Low"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/New mob.auto b/src/main/deploy/pathplanner/autos/New mob.auto
new file mode 100644
index 00000000..268147bb
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/New mob.auto	
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score 1 G L4.auto b/src/main/deploy/pathplanner/autos/Score 1 G L4.auto
new file mode 100644
index 00000000..edfea468
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score 1 G L4.auto	
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near G"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to G"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back G"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score D L4.auto b/src/main/deploy/pathplanner/autos/Score D L4.auto
new file mode 100644
index 00000000..8524c5ef
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score D L4.auto	
@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near D"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to D"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back D"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto
new file mode 100644
index 00000000..394eeb7d
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score F L4 + Y + C L4 .auto	
@@ -0,0 +1,182 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near F"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to F"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back F"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "F to Source X"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Elevator to 0.8 meter"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Arm for Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Elevator to 0.4 meter"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Source X to Near C"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to C"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back C"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score F L4.auto b/src/main/deploy/pathplanner/autos/Score F L4.auto
new file mode 100644
index 00000000..ab2a2366
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score F L4.auto	
@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near F"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to F"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back F"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score G L4.auto b/src/main/deploy/pathplanner/autos/Score G L4.auto
deleted file mode 100644
index f36d58e9..00000000
--- a/src/main/deploy/pathplanner/autos/Score G L4.auto	
+++ /dev/null
@@ -1,56 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 0
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Start to Near G"
-          }
-        },
-        {
-          "type": "parallel",
-          "data": {
-            "commands": [
-              {
-                "type": "named",
-                "data": {
-                  "name": "Move Arm High"
-                }
-              },
-              {
-                "type": "named",
-                "data": {
-                  "name": "Move Elevator"
-                }
-              }
-            ]
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Go Straight 0.3 Meter"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Move Arm Low"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto
new file mode 100644
index 00000000..8a4bedf6
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score I L4 + Y + L L4 .auto	
@@ -0,0 +1,182 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near I"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to I"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back I"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "I to Source Y"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Elevator to 0.8 meter"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Arm for Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Elevator to 0.4 meter"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Source Y to Near L"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to L"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back L"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score I L4.auto b/src/main/deploy/pathplanner/autos/Score I L4.auto
new file mode 100644
index 00000000..b676e942
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score I L4.auto	
@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near I"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to I"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back I"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto
new file mode 100644
index 00000000..909a2d8f
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score J L4 + Y + L L4 .auto	
@@ -0,0 +1,182 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near J"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to J"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back J"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "J to Source Y"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Elevator to 0.8 meter"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Move Arm for Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Elevator to 0.4 meter"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Source Y to Near L"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to L"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back L"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Score J L4.auto b/src/main/deploy/pathplanner/autos/Score J L4.auto
new file mode 100644
index 00000000..2836fd08
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Score J L4.auto	
@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Near J"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Move Elevator to 1.0 meter"
+                      }
+                    },
+                    {
+                      "type": "parallel",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Arm 75 degrees"
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Move Elevator to 1.553 meter"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Get to J"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Move Arm 0 degrees"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Back J"
+          }
+        }
+      ]
+    }
+  },
+  "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 af0de35c..f827e092 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":17.548,"y":8.052},"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,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,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,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,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,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,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,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,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,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,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,false,false,true,true],[true,true,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,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,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,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,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,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,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,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,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,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,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,false,false,false,true,true],[true,true,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,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,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,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,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,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,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,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,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,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":17.548,"y":8.052},"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,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,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,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,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,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,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,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,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,false,false,false,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,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,false,false,false,false,false,false,false,false,false,false,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,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,true,true,true,true,true,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,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,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,false,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,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,false,false,false,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,false,false,false,false,false,false,false,false,false,false,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,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,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,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,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,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,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,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,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,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Straight.path b/src/main/deploy/pathplanner/paths/Back C.path
similarity index 62%
rename from src/main/deploy/pathplanner/paths/Straight.path
rename to src/main/deploy/pathplanner/paths/Back C.path
index 6a81612c..e5ffa71a 100644
--- a/src/main/deploy/pathplanner/paths/Straight.path
+++ b/src/main/deploy/pathplanner/paths/Back C.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.0,
-        "y": 1.0
+        "x": 3.729374999999999,
+        "y": 3.0627414772727275
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.0,
-        "y": 1.0
+        "x": 3.685445168296228,
+        "y": 2.816631400505281
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "C Reef"
     },
     {
       "anchor": {
-        "x": 2.0,
-        "y": 1.0
+        "x": 3.5199715909090905,
+        "y": 2.6937926136363637
       },
       "prevControl": {
-        "x": 1.0,
-        "y": 1.0
+        "x": 3.528294263800622,
+        "y": 2.9436540414637506
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 2.0,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 57.27
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 57.27
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back D.path b/src/main/deploy/pathplanner/paths/Back D.path
new file mode 100644
index 00000000..621c8ad6
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back D.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.9966025641025635,
+        "y": 2.8794230769230773
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8725664285837027,
+        "y": 2.6179661616351546
+      },
+      "isLocked": false,
+      "linkedName": "D Reef"
+    },
+    {
+      "anchor": {
+        "x": 3.821153846153846,
+        "y": 2.5904487179487172
+      },
+      "prevControl": {
+        "x": 3.9072673463703698,
+        "y": 2.8251495170109675
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back F.path b/src/main/deploy/pathplanner/paths/Back F.path
new file mode 100644
index 00000000..cf1474a9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back F.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.2350852272727275,
+        "y": 3.0627414772727275
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.469771343104384,
+        "y": 2.624743314985711
+      },
+      "isLocked": false,
+      "linkedName": "F Reef"
+    },
+    {
+      "anchor": {
+        "x": 5.394630681818183,
+        "y": 2.77356534090909
+      },
+      "prevControl": {
+        "x": 5.275994919451355,
+        "y": 2.9936234148069625
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near F"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 122.66091272167375
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 122.66
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back G.path b/src/main/deploy/pathplanner/paths/Back G.path
new file mode 100644
index 00000000..5a16d5e2
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back G.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.753607954545455,
+        "y": 3.85
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.025529390821386,
+        "y": 3.840199234078392
+      },
+      "isLocked": false,
+      "linkedName": "G Reef"
+    },
+    {
+      "anchor": {
+        "x": 6.2,
+        "y": 3.85
+      },
+      "prevControl": {
+        "x": 5.950621361185062,
+        "y": 3.8676151781940615
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near G"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back I.path b/src/main/deploy/pathplanner/paths/Back I.path
new file mode 100644
index 00000000..7c3d6447
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back I.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.2350852272727275,
+        "y": 4.9972301136363635
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.3576505371808665,
+        "y": 5.215123997917501
+      },
+      "isLocked": false,
+      "linkedName": "I Reef"
+    },
+    {
+      "anchor": {
+        "x": 5.4046022727272724,
+        "y": 5.28640625
+      },
+      "prevControl": {
+        "x": 5.248428510838665,
+        "y": 5.091189047639243
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near I"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back J.path b/src/main/deploy/pathplanner/paths/Back J.path
new file mode 100644
index 00000000..7244bb25
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back J.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.925965909090909,
+        "y": 5.206633522727272
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.0454673590505665,
+        "y": 5.4262226924454975
+      },
+      "isLocked": false,
+      "linkedName": "J Reef"
+    },
+    {
+      "anchor": {
+        "x": 5.090576923076923,
+        "y": 5.480192307692307
+      },
+      "prevControl": {
+        "x": 4.968184609677482,
+        "y": 5.251429232511448
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near J"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Back L.path b/src/main/deploy/pathplanner/paths/Back L.path
new file mode 100644
index 00000000..77506dfb
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Back L.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.7194034090909094,
+        "y": 4.9972301136363635
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.512002099417438,
+        "y": 5.246749642274254
+      },
+      "isLocked": false,
+      "linkedName": "L Reef"
+    },
+    {
+      "anchor": {
+        "x": 3.387692307692307,
+        "y": 5.542115384615385
+      },
+      "prevControl": {
+        "x": 3.5822727940278267,
+        "y": 5.327860587160394
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near L"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -57.265
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -57.265
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/F to Source X.path b/src/main/deploy/pathplanner/paths/F to Source X.path
new file mode 100644
index 00000000..d04803e9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/F to Source X.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.394630681818183,
+        "y": 2.77356534090909
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.676676136363636,
+        "y": 2.2251278409090913
+      },
+      "isLocked": false,
+      "linkedName": "Near F"
+    },
+    {
+      "anchor": {
+        "x": 1.3132692307692304,
+        "y": 0.9701282051282044
+      },
+      "prevControl": {
+        "x": 2.529803321678322,
+        "y": 1.25930434149184
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Source X"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 53.84
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 122.66091272167375
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/G to Source Y.path b/src/main/deploy/pathplanner/paths/G to Source Y.path
new file mode 100644
index 00000000..8d0aaa8a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/G to Source Y.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.2,
+        "y": 3.85
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.109744318181819,
+        "y": 6.253792613636363
+      },
+      "isLocked": false,
+      "linkedName": "Near G"
+    },
+    {
+      "anchor": {
+        "x": 1.1267897727272727,
+        "y": 7.051377840909091
+      },
+      "prevControl": {
+        "x": 3.9886363636363633,
+        "y": 6.522883522727274
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Source Y"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.841814560191686
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to C.path b/src/main/deploy/pathplanner/paths/Get to C.path
new file mode 100644
index 00000000..00e35ded
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to C.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5631410256410248,
+        "y": 2.7658974358974366
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8289600484680144,
+        "y": 3.2496381820489777
+      },
+      "isLocked": false,
+      "linkedName": "Near C"
+    },
+    {
+      "anchor": {
+        "x": 3.729374999999999,
+        "y": 3.0627414772727275
+      },
+      "prevControl": {
+        "x": 3.6085933269601185,
+        "y": 2.8424690993271704
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "C Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to D.path b/src/main/deploy/pathplanner/paths/Get to D.path
new file mode 100644
index 00000000..cb7d7398
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to D.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.490897435897436,
+        "y": 2.9826282051282056
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8830769230769224,
+        "y": 2.88974358974359
+      },
+      "isLocked": false,
+      "linkedName": "Near D"
+    },
+    {
+      "anchor": {
+        "x": 3.9966025641025635,
+        "y": 2.8794230769230773
+      },
+      "prevControl": {
+        "x": 3.604423076923076,
+        "y": 2.8691025641025636
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "D Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 25.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to F.path b/src/main/deploy/pathplanner/paths/Get to F.path
new file mode 100644
index 00000000..f7f8a718
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to F.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.394630681818183,
+        "y": 2.77356534090909
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.218864011662972,
+        "y": 2.953868223607026
+      },
+      "isLocked": false,
+      "linkedName": "Near F"
+    },
+    {
+      "anchor": {
+        "x": 5.2350852272727275,
+        "y": 3.0627414772727275
+      },
+      "prevControl": {
+        "x": 5.4233322918817635,
+        "y": 2.8787143928800902
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "F Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 122.66
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 122.66091272167375
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to G.path b/src/main/deploy/pathplanner/paths/Get to G.path
index 34f807e9..c3f2fb2b 100644
--- a/src/main/deploy/pathplanner/paths/Get to G.path	
+++ b/src/main/deploy/pathplanner/paths/Get to G.path	
@@ -4,28 +4,28 @@
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.873
+        "y": 3.85
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.95,
-        "y": 3.873
+        "x": 5.928238356816237,
+        "y": 3.839672387595645
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Near G"
     },
     {
       "anchor": {
-        "x": 5.713721590909091,
-        "y": 3.873
+        "x": 5.753607954545455,
+        "y": 3.85
       },
       "prevControl": {
-        "x": 6.331960227272727,
-        "y": 3.8903835227272725
+        "x": 6.002823135925256,
+        "y": 3.8697937709856585
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "G Reef"
     }
   ],
   "rotationTargets": [],
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 2.0,
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to I.path b/src/main/deploy/pathplanner/paths/Get to I.path
new file mode 100644
index 00000000..0f4d2056
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to I.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.4046022727272724,
+        "y": 5.28640625
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.252248187767877,
+        "y": 5.088193655051583
+      },
+      "isLocked": false,
+      "linkedName": "Near I"
+    },
+    {
+      "anchor": {
+        "x": 5.2350852272727275,
+        "y": 4.9972301136363635
+      },
+      "prevControl": {
+        "x": 5.397808250912335,
+        "y": 5.193762371483878
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "I Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to J.path b/src/main/deploy/pathplanner/paths/Get to J.path
new file mode 100644
index 00000000..cdc106fa
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to J.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.090576923076923,
+        "y": 5.480192307692307
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.9405769230769225,
+        "y": 5.280192307692307
+      },
+      "isLocked": false,
+      "linkedName": "Near J"
+    },
+    {
+      "anchor": {
+        "x": 4.925965909090909,
+        "y": 5.206633522727272
+      },
+      "prevControl": {
+        "x": 5.064640958147216,
+        "y": 5.414646096311733
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "J Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Get to L.path b/src/main/deploy/pathplanner/paths/Get to L.path
new file mode 100644
index 00000000..b7fb74db
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Get to L.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.387692307692307,
+        "y": 5.542115384615385
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.5435712961286225,
+        "y": 5.28103274057065
+      },
+      "isLocked": false,
+      "linkedName": "Near L"
+    },
+    {
+      "anchor": {
+        "x": 3.7194034090909094,
+        "y": 4.9972301136363635
+      },
+      "prevControl": {
+        "x": 3.565097644575375,
+        "y": 5.231798745830039
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "L Reef"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -57.265
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -57.265
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Go Forward.path b/src/main/deploy/pathplanner/paths/Go Forward.path
index 45546292..de3846ef 100644
--- a/src/main/deploy/pathplanner/paths/Go Forward.path	
+++ b/src/main/deploy/pathplanner/paths/Go Forward.path	
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 2.0,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
diff --git a/src/main/deploy/pathplanner/paths/I to Source Y.path b/src/main/deploy/pathplanner/paths/I to Source Y.path
new file mode 100644
index 00000000..ffdeb49f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/I to Source Y.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.4046022727272724,
+        "y": 5.28640625
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.670994318176833,
+        "y": 6.453082386358649
+      },
+      "isLocked": false,
+      "linkedName": "Near I"
+    },
+    {
+      "anchor": {
+        "x": 1.1267897727272727,
+        "y": 7.051377840909091
+      },
+      "prevControl": {
+        "x": 0.8320180915273928,
+        "y": 7.139879250452775
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Source Y"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.841814560191686
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/J to Source Y.path b/src/main/deploy/pathplanner/paths/J to Source Y.path
new file mode 100644
index 00000000..c3530530
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/J to Source Y.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.090576923076923,
+        "y": 5.480192307692307
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.356968968526483,
+        "y": 6.6468684440509564
+      },
+      "isLocked": false,
+      "linkedName": "Near J"
+    },
+    {
+      "anchor": {
+        "x": 1.1267897727272727,
+        "y": 7.051377840909091
+      },
+      "prevControl": {
+        "x": 0.8320180915273928,
+        "y": 7.139879250452775
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Source Y"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.841814560191686
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
index 9202a4ab..37e02580 100644
--- a/src/main/deploy/pathplanner/paths/New Path.path	
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.122556818181818,
-        "y": 3.950213068181818
+        "x": 7.678125,
+        "y": 6.662485795454545
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.661022727272727,
-        "y": 5.50578125
+        "x": 5.384659090909091,
+        "y": 6.183849431818182
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.8946022727272724,
-        "y": 6.5
+        "x": 4.427386363636364,
+        "y": 5.934559659090909
       },
       "prevControl": {
-        "x": 4.557017045454547,
-        "y": 6.639602272727273
+        "x": 5.424545454545455,
+        "y": 6.1539346590909085
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.2672,
-    "maxAcceleration": 9.466478,
-    "maxAngularVelocity": 30.0,
+    "maxVelocity": 2.5,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 141.61670272897086
+    "rotation": 172.48157312055784
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -178.85423716182487
+    "rotation": -178.94559549647815
   },
   "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source X to Near C.path b/src/main/deploy/pathplanner/paths/Source X to Near C.path
new file mode 100644
index 00000000..31db1a25
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Source X to Near C.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3132692307692304,
+        "y": 0.9701282051282044
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.526478329534876,
+        "y": 1.1006727576899302
+      },
+      "isLocked": false,
+      "linkedName": "Source X"
+    },
+    {
+      "anchor": {
+        "x": 3.5631410256410248,
+        "y": 2.7658974358974366
+      },
+      "prevControl": {
+        "x": 3.2232269472838215,
+        "y": 2.4843124399988734
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near C"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.27
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 53.84
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source Y to Near L.path b/src/main/deploy/pathplanner/paths/Source Y to Near L.path
new file mode 100644
index 00000000..70f77b16
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Source Y to Near L.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1267897727272727,
+        "y": 7.051377840909091
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2707016981542534,
+        "y": 6.181233309988742
+      },
+      "isLocked": false,
+      "linkedName": "Source Y"
+    },
+    {
+      "anchor": {
+        "x": 3.387692307692307,
+        "y": 5.542115384615385
+      },
+      "prevControl": {
+        "x": 3.0749971691662767,
+        "y": 5.807238017068298
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near L"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -57.265
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.841814560191686
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near D.path b/src/main/deploy/pathplanner/paths/Start to Near D.path
new file mode 100644
index 00000000..050447cb
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Start to Near D.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.857613636363637,
+        "y": 1.9259801136363626
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.22474358974359,
+        "y": 1.5067948717948723
+      },
+      "isLocked": false,
+      "linkedName": "Left Start"
+    },
+    {
+      "anchor": {
+        "x": 3.490897435897436,
+        "y": 2.9826282051282056
+      },
+      "prevControl": {
+        "x": 2.789102564102564,
+        "y": 1.3932692307692307
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near D"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 25.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near F.path b/src/main/deploy/pathplanner/paths/Start to Near F.path
new file mode 100644
index 00000000..6de63a0f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Start to Near F.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.857613636363637,
+        "y": 1.9259801136363626
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.89811462363063,
+        "y": 2.117009249713284
+      },
+      "isLocked": false,
+      "linkedName": "Left Start"
+    },
+    {
+      "anchor": {
+        "x": 5.394630681818183,
+        "y": 2.77356534090909
+      },
+      "prevControl": {
+        "x": 5.893210227272728,
+        "y": 2.1952130681818187
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near F"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 122.66091272167375
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near G.path b/src/main/deploy/pathplanner/paths/Start to Near G.path
index ade470d4..edbe03bd 100644
--- a/src/main/deploy/pathplanner/paths/Start to Near G.path	
+++ b/src/main/deploy/pathplanner/paths/Start to Near G.path	
@@ -8,24 +8,24 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.997,
-        "y": 3.99
+        "x": 6.970945292591921,
+        "y": 3.8734607488009942
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Center Start"
     },
     {
       "anchor": {
         "x": 6.2,
-        "y": 3.873
+        "y": 3.85
       },
       "prevControl": {
-        "x": 7.115994318181788,
-        "y": 3.8075596590909093
+        "x": 7.006960298309069,
+        "y": 3.9368660079538973
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Near G"
     }
   ],
   "rotationTargets": [],
@@ -33,9 +33,9 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 2.0,
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 90.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near I.path b/src/main/deploy/pathplanner/paths/Start to Near I.path
new file mode 100644
index 00000000..cc9f00af
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Start to Near I.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.867585227272728,
+        "y": 6.183849431818182
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.891051929760262,
+        "y": 6.002461229032797
+      },
+      "isLocked": false,
+      "linkedName": "Right Start"
+    },
+    {
+      "anchor": {
+        "x": 5.4046022727272724,
+        "y": 5.28640625
+      },
+      "prevControl": {
+        "x": 5.604034090909091,
+        "y": 5.874730113636364
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near I"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Start to Near J.path b/src/main/deploy/pathplanner/paths/Start to Near J.path
new file mode 100644
index 00000000..3cb338dc
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Start to Near J.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.867585227272728,
+        "y": 6.183849431818182
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.7906534090909085,
+        "y": 6.443110795454545
+      },
+      "isLocked": false,
+      "linkedName": "Right Start"
+    },
+    {
+      "anchor": {
+        "x": 5.090576923076923,
+        "y": 5.480192307692307
+      },
+      "prevControl": {
+        "x": 5.459525786713287,
+        "y": 6.537180944055944
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Near J"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 45.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.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 1aa11407..c20d16ae 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -1,20 +1,20 @@
 {
-  "robotWidth": 0.711,
-  "robotLength": 0.7112,
+  "robotWidth": 0.7493,
+  "robotLength": 0.7493,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 2.5,
-  "defaultMaxAccel": 2.5,
-  "defaultMaxAngVel": 2.0,
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 45.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
   "robotMass": 74.088,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
+  "driveWheelRadius": 0.0508,
   "driveGearing": 5.143,
-  "maxDriveSpeed": 5.45,
+  "maxDriveSpeed": 5.25,
   "driveMotorType": "NEO",
   "driveCurrentLimit": 60.0,
   "wheelCOF": 1.2,
diff --git a/src/main/deploy/yagsl/comp/modules/backleft.json b/src/main/deploy/yagsl/comp/modules/backleft.json
index 37510797..15bdef3b 100644
--- a/src/main/deploy/yagsl/comp/modules/backleft.json
+++ b/src/main/deploy/yagsl/comp/modules/backleft.json
@@ -3,7 +3,7 @@
     "front": -12.125,
     "left": 12.125
   },
-  "absoluteEncoderOffset": 277.383,
+  "absoluteEncoderOffset": 268.383,
   "drive": {
     "type": "sparkmax_neo",
     "id": 12,
diff --git a/src/main/deploy/yagsl/comp/modules/backright.json b/src/main/deploy/yagsl/comp/modules/backright.json
index 13316823..5d3f0bf2 100644
--- a/src/main/deploy/yagsl/comp/modules/backright.json
+++ b/src/main/deploy/yagsl/comp/modules/backright.json
@@ -3,7 +3,7 @@
     "front": -12.125,
     "left": -12.125
   },
-  "absoluteEncoderOffset": 333.676,
+  "absoluteEncoderOffset": 336.445,
   "drive": {
     "type": "sparkmax_neo",
     "id": 14,
diff --git a/src/main/deploy/yagsl/comp/modules/frontleft.json b/src/main/deploy/yagsl/comp/modules/frontleft.json
index 40594f82..6e09bc44 100644
--- a/src/main/deploy/yagsl/comp/modules/frontleft.json
+++ b/src/main/deploy/yagsl/comp/modules/frontleft.json
@@ -3,7 +3,7 @@
     "front": 12.125,
     "left": 12.125
   },
-  "absoluteEncoderOffset": 251.016,
+  "absoluteEncoderOffset": 247.850,
   "drive": {
     "type": "sparkmax_neo",
     "id": 10,
diff --git a/src/main/deploy/yagsl/comp/modules/frontright.json b/src/main/deploy/yagsl/comp/modules/frontright.json
index 7c711308..4d886a60 100644
--- a/src/main/deploy/yagsl/comp/modules/frontright.json
+++ b/src/main/deploy/yagsl/comp/modules/frontright.json
@@ -3,7 +3,7 @@
     "front": 12.125,
     "left": -12.125
   },
-  "absoluteEncoderOffset": 41.836,
+  "absoluteEncoderOffset": 39.200,
   "drive": {
     "type": "sparkmax_neo",
     "id": 16,
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 21e08ce3..7371c9c6 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -20,7 +20,6 @@ public class RobotContainer {
 
   public RobotContainer() {
     String robotName = "UNKNOWN";
-
     // Load robot name from configuration file
     // Check if the robot is running in simulation
     if (RobotBase.isSimulation()) {
diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
new file mode 100644
index 00000000..0d343ddd
--- /dev/null
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToPositionX.java
@@ -0,0 +1,62 @@
+package frc.robot.commands.common.drive;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants;
+import frc.robot.subsystems.interfaces.Drive;
+import java.util.function.DoubleSupplier;
+
+public class DriveToPositionX extends Command {
+  Drive drive;
+  DoubleSupplier distanceMeters;
+  Pose2d targetPose;
+
+  public DriveToPositionX(Drive drive, DoubleSupplier distanceMeters) {
+    this.drive = drive;
+    this.distanceMeters = distanceMeters;
+    addRequirements((Subsystem) drive);
+  }
+
+  @Override
+  public void initialize() {
+    System.out.println("  START: " + this.getClass().getSimpleName());
+    double d = this.distanceMeters.getAsDouble();
+    targetPose = drive.getPose().transformBy(new Transform2d(d, 0, new Rotation2d()));
+  }
+
+  @Override
+  public void execute() {
+
+    drive.runVelocity(
+        new ChassisSpeeds(
+            Math.signum(distanceMeters.getAsDouble()) * (drive.getMaxLinearSpeed()), 0, 0));
+  }
+
+  @Override
+  public boolean isFinished() {
+    // double currentX = drive.getPose().getX();
+
+    // double targetX = targetPose.getX();
+    double distanceLeft =
+        new Translation2d().getDistance(drive.getPose().relativeTo(targetPose).getTranslation());
+    System.out.println("Distance Away: " + distanceLeft);
+    return Math.abs(distanceLeft) <= 0.2;
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    if (interrupted) {
+      System.err.println("INTERRUPTED: " + this.getClass().getSimpleName());
+    }
+
+    drive.runVelocity(new ChassisSpeeds());
+    if (Constants.debugCommands) {
+      System.out.println("  END: " + this.getClass().getSimpleName());
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java b/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
index 187aafcf..2c07cf42 100644
--- a/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
+++ b/src/main/java/frc/robot/commands/common/drive/DriveToYaw.java
@@ -30,38 +30,30 @@ public DriveToYaw(Drive drive, DoubleSupplier yawDegrees) {
 
   @Override
   public void initialize() {
+    // System.out.println("  START: " + this.getClass().getSimpleName());
     targetYaw = this.yawDegrees.getAsDouble();
     turnPID.reset();
     turnPID.setSetpoint(targetYaw);
     timer.reset();
-    if (Constants.debugCommands) {
-      System.out.println(
-          "START: "
-              + this.getClass().getSimpleName()
-              + " yaw: "
-              + targetYaw
-              + " currentYaw: "
-              + drive.getAngle());
-    }
   }
 
   @Override
   public void execute() {
     double rotate = turnPID.calculate(drive.getAngle());
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, rotate * drive.getMaxAngularSpeed());
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, rotate * (drive.getMaxAngularSpeed() / 8));
     drive.runVelocity(speeds);
+    targetYaw = this.yawDegrees.getAsDouble();
+    double yawLeft = targetYaw - drive.getAngle();
+    System.out.println("Angle Away: " + yawLeft);
   }
 
   @Override
   public boolean isFinished() {
     if (turnPID.atSetpoint()) {
-      if (timer.get() >= DriveBase.Constants.pidSettlingTimeInSeconds) {
-        return true;
-      }
+      return true;
     } else {
-      timer.reset();
+      return false;
     }
-    return false;
   }
 
   @Override
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
index 559cb05b..1588d91e 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfig.java
@@ -1,5 +1,6 @@
 package frc.robot.config.game.reefscape2025;
 
+import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.controller.ArmFeedforward;
@@ -205,18 +206,23 @@ public RobotConfig(
   public void configureBindings() {
     if (Robot.isSimulation()) {
       vision.enableSimulation(() -> RobotConfig.drive.getPose(), true);
+
+      // HACK just to verify autos are visible without connecting to robot
+      RobotConfig.autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
     }
 
     // Send vision-based odometry measurements to drive's odometry calculations
     vision.setVisionMeasurementConsumer(drive::addVisionMeasurement);
 
-    DriveControls.setupController(drive, mainController);
+    CoralArmControls.setupController(
+        coralArm, assistController); // move up to get reset encoder command
+    DriveControls.setupController(drive, elevator, coralArm, mainController);
+    DriveControls.setupAssistantController(drive, assistController);
     DriverAssistControls.setupController(elevator, coralArm, climberArm, assistController);
     DriverControls.setupController(elevator, coralArm, climberArm, mainController);
     PitControls.setupPitControls(elevator, coralArm, climberArm);
     CoralArmControls.setupController(coralArm, mainController);
     ElevatorControls.setupController(elevator, mainController);
-    CoralArmControls.setupController(coralArm, assistController);
     ElevatorControls.setupController(elevator, assistController);
     ClimberArmControls.setupController(climberArm, mainController);
 
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
index 89618292..443ac55c 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java
@@ -1,5 +1,8 @@
 package frc.robot.config.game.reefscape2025;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.PIDController;
@@ -13,6 +16,8 @@
 import edu.wpi.first.wpilibj.LEDPattern;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
+import frc.robot.commands.common.arm.ArmToPosition;
+import frc.robot.commands.common.elevator.ElevatorToPosition;
 import frc.robot.io.implementations.motor.MotorIOBase.MotorIOBaseSettings;
 import frc.robot.io.implementations.motor.MotorIOTalonFx;
 import frc.robot.io.implementations.motor.MotorIOTalonFx.TalonFxSettings;
@@ -32,13 +37,13 @@ public class RobotConfigComp extends RobotConfig {
   private final AddressableLEDBuffer ledBuffer;
 
   public RobotConfigComp() {
-    super(false, true, false, false, false, true, true);
+    super(false, false, false, false, false, true, true);
 
     // Comp has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
     Drive.Constants.rotatePidKi = 0.0;
     Drive.Constants.rotatePidKd = 0.0;
-    DriveBase.Constants.rotatePidErrorInDegrees = 1;
+    DriveBase.Constants.rotatePidErrorInDegrees = 2;
     drive = new DriveSwerveYAGSL("yagsl/comp");
 
     // Cameras
@@ -48,45 +53,56 @@ public RobotConfigComp() {
             new Transform3d(
                 new Translation3d(
                     Units.inchesToMeters(0.295),
-                    Units.inchesToMeters(-11.443),
+                    Units.inchesToMeters(0),
                     Units.inchesToMeters(39.663)),
                 new Rotation3d(
-                    Units.degreesToRadians(12),
+                    Units.degreesToRadians(0),
                     Units.degreesToRadians(-33),
-                    Units.degreesToRadians(170)))));
+                    Units.degreesToRadians(180)))));
+    // vision.addCamera(
+    //     new Camera(
+    //         "left_cam", // left
+    //         new Transform3d(
+    //             new Translation3d(
+    //                 Units.inchesToMeters(3.250),
+    //                 Units.inchesToMeters(13.592),
+    //                 Units.inchesToMeters(7.201)),
+    //             new Rotation3d(0.0, Units.degreesToRadians(-5.0),
+    // Units.degreesToRadians(90.0)))));
     vision.addCamera(
         new Camera(
             "left_cam", // left
             new Transform3d(
                 new Translation3d(
-                    Units.inchesToMeters(3.250),
-                    Units.inchesToMeters(13.592),
-                    Units.inchesToMeters(7.201)),
-                new Rotation3d(0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(90.0)))));
+                    Units.inchesToMeters(9.343),
+                    Units.inchesToMeters(11.974),
+                    Units.inchesToMeters(11.110)),
+                new Rotation3d(0.0, Units.degreesToRadians(5.0), Units.degreesToRadians(-22)))));
 
-    vision.addCamera(
-        new Camera(
-            "right_cam", // right
-            new Transform3d(
-                new Translation3d(
-                    Units.inchesToMeters(3.250),
-                    Units.inchesToMeters(-13.592),
-                    Units.inchesToMeters(7.201)),
-                new Rotation3d(0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(270.0)))));
+    // vision.addCamera(
+    //     new Camera(
+    //         "right_cam", // right
+    //         new Transform3d(
+    //             new Translation3d(
+    //                 Units.inchesToMeters(3.250),
+    //                 Units.inchesToMeters(-13.592),
+    //                 Units.inchesToMeters(7.201)),
+    //             new Rotation3d(0.0, Units.degreesToRadians(-5.0),
+    // Units.degreesToRadians(270.0)))));
     vision.addCamera(
         new Camera(
             "front_cam", // front
             new Transform3d(
                 new Translation3d(
-                    Units.inchesToMeters(13.592),
-                    Units.inchesToMeters(2.75),
-                    Units.inchesToMeters(7.201)),
-                new Rotation3d(0.0, Units.degreesToRadians(-5.0), 0.0))));
+                    Units.inchesToMeters(9.343),
+                    Units.inchesToMeters(-11.974),
+                    Units.inchesToMeters(11.110)),
+                new Rotation3d(0.0, Units.degreesToRadians(5.0), Units.degreesToRadians(22)))));
 
     // Elevator
     {
       MotorIOBaseSettings motorSettings = new MotorIOBaseSettings();
-      motorSettings.motor.gearing = 9; /* 2x 3:1 gear boxes */
+      motorSettings.motor.gearing = 5; /* 1x 5:1 gear boxes */
       motorSettings.motor.inverted = true;
       // TODO: Get the DIO ports
       // motorSettings.forwardLimitChannel = 7;
@@ -100,7 +116,8 @@ public RobotConfigComp() {
       ElevatorSettings elevatorSettings = new ElevatorSettings();
       elevatorSettings.minHeightInMeters = 0.09 + 0.02;
       elevatorSettings.maxHeightInMeters = 0.02 + 0.85 + 0.76;
-      elevatorSettings.startingHeightInMeters = 0.3; // The elevator height when piece is in intake
+      elevatorSettings.startingHeightInMeters =
+          0.345; // The elevator height when piece is in intake
       elevatorSettings.color = new Color8Bit(Color.kSilver);
       elevatorSettings.feedforward =
           new ElevatorFeedforward(0.010472, 0.17328, 0.16928, 0.010615); // SysID 2025-02-28
@@ -157,7 +174,7 @@ public RobotConfigComp() {
       CoralArmControls.Constants.autoZeroSettings.minResetCurrent = 0.5;
       CoralArmControls.Constants.autoZeroSettings.resetPositionRad =
           Units.degreesToRadians(
-              armSettings.minAngleInDegrees - 10); // We have an offest about 15 degrees
+              armSettings.minAngleInDegrees); // We have an offest about 15 degrees
       CoralArmControls.Constants.autoZeroSettings.initialReverseDuration =
           0.0; // Set the seconds of reverse before zero. Set to zero if there shound be no reverse
 
@@ -167,7 +184,7 @@ public RobotConfigComp() {
               new MotorIOTalonFx(motorSettings, talonFxSettings), "Coral", armSettings);
     }
 
-    // // climber
+    // climber
     // {
     //   MotorIOBaseSettings motorSettings = new MotorIOBaseSettings();
     //   // 25:1 gear box ratio
@@ -179,7 +196,7 @@ public RobotConfigComp() {
 
     //   SimpleMotorSettings simpleMotorSettings = new SimpleMotorSettings();
     //   simpleMotorSettings.minPositionInRads = 0;
-    //   simpleMotorSettings.maxPositionInRads = 36.1;
+    //   simpleMotorSettings.maxPositionInRads = 38.06;
     //   simpleMotorSettings.startingPositionInRads = simpleMotorSettings.minPositionInRads;
     //   simpleMotorSettings.color = new Color8Bit(Color.kRed);
     //   simpleMotorSettings.feedforward = new SimpleMotorFeedforward(0, 0, 0);
@@ -216,5 +233,27 @@ public RobotConfigComp() {
       led.setData(ledBuffer);
       led.start();
     }
+
+    // Auto(s)
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.5 meter", new ElevatorToPosition(elevator, () -> 0.5));
+    NamedCommands.registerCommand(
+        "Move Elevator to 1.0 meter", new ElevatorToPosition(elevator, () -> 1.0));
+    NamedCommands.registerCommand(
+        "Move Elevator to 1.553 meter", new ElevatorToPosition(elevator, () -> 1.553));
+    NamedCommands.registerCommand(
+        "Move Arm 75 degrees", new ArmToPosition(coralArm, () -> 70).withTimeout(1.5));
+    NamedCommands.registerCommand(
+        "Move Arm 0 degrees", new ArmToPosition(coralArm, () -> 0).withTimeout(1.0));
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.8 meter", new ElevatorToPosition(elevator, () -> 0.8));
+    NamedCommands.registerCommand(
+        "Move Elevator to 0.4 meter", new ElevatorToPosition(elevator, () -> 0.354));
+    NamedCommands.registerCommand(
+        "Move Arm for Intake", new ArmToPosition(coralArm, () -> -90).withTimeout(0));
+    autoChooser = AutoBuilder.buildAutoChooser("Sit Still");
+
+    // Start webcam
+    CameraServer.startAutomaticCapture();
   }
 }
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
index a6bb58ce..2ade8ccf 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigNemo.java
@@ -29,7 +29,7 @@
 /* Override Nemo specific constants here */
 public class RobotConfigNemo extends RobotConfig {
   public RobotConfigNemo() {
-    super(false, true, true, false, false, true, true);
+    super(false, false, false, false, false, true, true);
 
     // Nemo has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
index 99258acf..7b8feaf9 100644
--- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
+++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigPhoenix.java
@@ -14,7 +14,7 @@
 /* Override Phoenix specific constants here */
 public class RobotConfigPhoenix extends RobotConfig {
   public RobotConfigPhoenix() {
-    super(false, true, false, true, true, true);
+    super(false, false, false, true, true, true);
 
     // Phoenix has a Swerve drive train
     Drive.Constants.rotatePidKp = 0.025;
diff --git a/src/main/java/frc/robot/io/interfaces/DriveIO.java b/src/main/java/frc/robot/io/interfaces/DriveIO.java
index 7eac4ea7..8f671f58 100644
--- a/src/main/java/frc/robot/io/interfaces/DriveIO.java
+++ b/src/main/java/frc/robot/io/interfaces/DriveIO.java
@@ -1,6 +1,9 @@
 package frc.robot.io.interfaces;
 
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import frc.robot.Robot;
 import org.littletonrobotics.junction.AutoLog;
 import swervelib.SwerveDrive;
 
@@ -11,7 +14,11 @@ public static class DriveIOInputs {
     public double poseX = 0.0;
     public double poseY = 0.0;
     public double poseRotInDegrees = 0.0;
-    public double distanceFromSpeaker = 0;
+    public Pose2d flippedPose;
+    public double flippedPoseX = 0.0;
+    public double flippedPoseY = 0.0;
+    public double flippedPoseRotInDegrees = 0.0;
+    public Translation3d currentAcceleration = new Translation3d();
   }
 
   /** Updates the set of loggable inputs. */
@@ -20,20 +27,13 @@ public void updateInputs(DriveIOInputs inputs, SwerveDrive swerveDrive) {
     inputs.poseX = inputs.pose.getTranslation().getX();
     inputs.poseY = inputs.pose.getTranslation().getY();
     inputs.poseRotInDegrees = inputs.pose.getRotation().getDegrees();
-    /*
-        if (!DevilBotState.isRedAlliance()) {
-          inputs.distanceFromSpeaker =
-              Math.sqrt(
-                  Math.pow(inputs.poseX - DriveBase.Constants.blueSpeakerX, 2)
-                      + Math.pow(inputs.poseY - DriveBase.Constants.speakerY, 2));
-        } else {
-          inputs.distanceFromSpeaker =
-              Math.sqrt(
-                  Math.pow(inputs.poseX - DriveBase.Constants.redSpeakerX, 2)
-                      + Math.pow(inputs.poseY - DriveBase.Constants.speakerY, 2));
-        }
-        ;
-    */
+    inputs.flippedPose = FlippingUtil.flipFieldPose(inputs.pose);
+    inputs.flippedPoseX = inputs.flippedPose.getTranslation().getX();
+    inputs.flippedPoseY = inputs.flippedPose.getTranslation().getY();
+    inputs.flippedPoseRotInDegrees = inputs.flippedPose.getRotation().getDegrees();
+    if (!Robot.isSimulation()) {
+      inputs.currentAcceleration = swerveDrive.getAccel().get();
+    }
   }
   // Other methods for controlling the drive subsystem...
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java b/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
index 3fef0340..dfe63bdd 100644
--- a/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/arm/ClimberArmControls.java
@@ -48,7 +48,6 @@ public static void setupController(SimpleMotor arm, CommandXboxController contro
             () -> arm.setTargetPosition(arm.getSettings().maxPositionInRads), armSubsystem));
     SmartDashboard.putData(
         armSubsystem.getName() + "/Commands/Climb",
-        new InstantCommand(
-            () -> arm.setTargetPosition(arm.getSettings().minPositionInRads), armSubsystem));
+        new InstantCommand(() -> arm.setTargetPosition(18.06 - 0.0873), armSubsystem));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/arm/CoralArmControls.java b/src/main/java/frc/robot/subsystems/controls/arm/CoralArmControls.java
index ef4d18a5..508aa5f8 100644
--- a/src/main/java/frc/robot/subsystems/controls/arm/CoralArmControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/arm/CoralArmControls.java
@@ -17,6 +17,8 @@ public static class Constants {
     public static MotorAutoResetEncoderSettings autoZeroSettings =
         new MotorAutoResetEncoderSettings();
     public static Command autoCalibrateCommand;
+    public static Command autoCalibrateCommand2;
+    public static Command autoCalibrateCommand3;
   }
 
   // RIGHT POV = up arm
@@ -40,9 +42,19 @@ public static void setupController(Arm arm, CommandXboxController controller) {
     /* Add Auto Zero */
     Constants.autoCalibrateCommand =
         new MotorAutoResetEncoderCommand((Motor) arm, Constants.autoZeroSettings);
+    Constants.autoCalibrateCommand2 =
+        new MotorAutoResetEncoderCommand((Motor) arm, Constants.autoZeroSettings);
+    Constants.autoCalibrateCommand3 =
+        new MotorAutoResetEncoderCommand((Motor) arm, Constants.autoZeroSettings);
     SmartDashboard.putData(
         motorSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm",
         Constants.autoCalibrateCommand);
+    SmartDashboard.putData(
+        motorSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm Driver",
+        Constants.autoCalibrateCommand2);
+    SmartDashboard.putData(
+        motorSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm Assist",
+        Constants.autoCalibrateCommand3);
 
     SmartDashboard.putData(
         motorSubsystem.getName() + "/Commands/Arm To -90", new ArmToPosition(arm, () -> -90));
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
index f5616c30..b7fa4cc1 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverAssistControls.java
@@ -8,8 +8,11 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.elevator.ElevatorToPosition;
+import frc.robot.commands.common.motor.MotorAutoResetEncoderCommand;
+import frc.robot.subsystems.controls.arm.CoralArmControls;
 import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Elevator;
+import frc.robot.subsystems.interfaces.Motor;
 import frc.robot.subsystems.interfaces.SimpleMotor;
 
 public class DriverAssistControls {
@@ -46,20 +49,26 @@ public static void setupController(
             new ElevatorToPosition(elevator, () -> 1.3)));
 
     Command prepareIntakeCoralCommand =
-        new SequentialCommandGroup(
-            new ArmToPosition(coralArm, () -> -90).withTimeout(0),
-            new ElevatorToPosition(elevator, () -> 0.8));
+        new SequentialCommandGroup(new ElevatorToPosition(elevator, () -> 0.8));
+
+    SubsystemBase armSubsystem = (SubsystemBase) coralArm;
     controller
         .b()
         .onTrue(
-            prepareIntakeCoralCommand.andThen(
-                new InstantCommand(
-                    () -> {
-                      DriverControls.Constants.prepareScoreSelctedIndex = 1;
-                      SmartDashboard.putNumber(
-                          "Driver " + "/Misc/Prepare Selection",
-                          DriverControls.Constants.prepareScoreSelctedIndex);
-                    })));
+            prepareIntakeCoralCommand
+                .andThen(
+                    (Command)
+                        SmartDashboard.getData(
+                            armSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm Assist"))
+                .andThen(
+                    new InstantCommand(
+                        () -> {
+                          DriverControls.Constants.prepareScoreSelctedIndex = 1;
+                          SmartDashboard.putString("Driver /Misc/Prepare Selection", "N/A");
+                        }))
+                .andThen(
+                    new MotorAutoResetEncoderCommand(
+                        (Motor) coralArm, CoralArmControls.Constants.autoZeroSettings)));
 
     controller
         .y()
@@ -67,9 +76,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 4;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L4");
                 }));
 
     controller
@@ -78,9 +85,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 3;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L3");
                 }));
 
     controller
@@ -89,9 +94,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 2;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L2");
                 }));
 
     // controller
@@ -114,8 +117,6 @@ public static void setupController(
     controller
         .rightBumper()
         .onTrue(
-            new InstantCommand(
-                () -> climber.setTargetPosition(climber.getSettings().minPositionInRads),
-                climberSubsystem));
+            new InstantCommand(() -> climber.setTargetPosition(18.06 - 0.175), climberSubsystem));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
index 5448a4a7..42e34742 100644
--- a/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/combination/DriverControls.java
@@ -38,26 +38,28 @@ public static void setupController(
             });
 
     Command prepareIntakeCoralCommand =
-        new SequentialCommandGroup(
-            new ArmToPosition(coralArm, () -> -90).withTimeout(0),
-            new ElevatorToPosition(elevator, () -> 0.8));
+        new SequentialCommandGroup(new ElevatorToPosition(elevator, () -> 0.8));
+    SubsystemBase armSubsystem = (SubsystemBase) coralArm;
     controller
         .b()
         .onTrue(
-            prepareIntakeCoralCommand.andThen(
-                new InstantCommand(
-                    () -> {
-                      DriverControls.Constants.prepareScoreSelctedIndex = 1;
-                      SmartDashboard.putNumber(
-                          "Driver " + "/Misc/Prepare Selection",
-                          DriverControls.Constants.prepareScoreSelctedIndex);
-                    })));
-
-    Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.35);
+            prepareIntakeCoralCommand
+                .andThen(
+                    (Command)
+                        SmartDashboard.getData(
+                            armSubsystem.getName() + "/Commands/Auto Calibrate Coral Arm Driver"))
+                .andThen(
+                    new InstantCommand(
+                        () -> {
+                          DriverControls.Constants.prepareScoreSelctedIndex = 1;
+                          SmartDashboard.putString("Driver /Misc/Prepare Selection", "N/A");
+                        })));
+
+    Command intakeCoralCommand = new ElevatorToPosition(elevator, () -> 0.345);
     Trigger scoreMode = new Trigger(() -> Constants.prepareScoreSelctedIndex >= 2);
-    controller.leftTrigger().and(scoreMode.negate()).and(ableToIntake).onTrue(intakeCoralCommand);
+    // controller.leftTrigger().and(scoreMode.negate()).and(ableToIntake).onTrue(intakeCoralCommand);
 
-    Command score = new ArmToPosition(coralArm, () -> 0);
+    Command score = new ArmToPosition(coralArm, () -> -10);
     controller.rightBumper().onTrue(score);
 
     Constants.prepareChooser.setDefaultOption("L2", 2);
@@ -74,9 +76,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 4;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L4");
                 }));
 
     controller
@@ -85,9 +85,7 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 3;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L3");
                 }));
 
     controller
@@ -96,44 +94,42 @@ public static void setupController(
             new InstantCommand(
                 () -> {
                   DriverControls.Constants.prepareScoreSelctedIndex = 2;
-                  SmartDashboard.putNumber(
-                      "Driver " + "/Misc/Prepare Selection",
-                      DriverControls.Constants.prepareScoreSelctedIndex);
+                  SmartDashboard.putString("Driver /Misc/Prepare Selection", "L2");
                 }));
 
     DriverControls.Constants.prepareChooser.onChange(
         (index) -> {
           DriverControls.Constants.prepareScoreSelctedIndex = index;
-          SmartDashboard.putNumber("Driver " + "/Misc/Prepare To Score Selection", index);
+          String desc;
+          if (index == 2) desc = "L2";
+          else if (index == 3) desc = "L3";
+          else desc = "L4";
+          SmartDashboard.putString("Driver /Misc/Prepare Selection", desc);
         });
 
-    SmartDashboard.putNumber(
-        "Driver " + "/Misc/Prepare To Score Selection", Constants.prepareChooser.getSelected());
+    SmartDashboard.putData("Driver " + "/Misc/Prepare To Score Chooser", Constants.prepareChooser);
+    SmartDashboard.putString("Driver /Misc/Prepare Selection", "L2");
 
     SmartDashboard.putData(
         "Driver " + "/Commands/Prepare To Score Command",
         getPrepareToScoreCommand(elevator, coralArm));
 
-    SmartDashboard.putData("Driver " + "/Misc/Prepare To Score Chooser", Constants.prepareChooser);
-
-    // climber
-    SubsystemBase climberSubsystem = (SubsystemBase) climber;
-    // prepare to climb
-    controller
-        .leftBumper()
-        .onTrue(
-            new InstantCommand(
-                () -> climber.setTargetPosition(climber.getSettings().maxPositionInRads),
-                climberSubsystem));
-
-    // climb
-    controller
-        .rightBumper()
-        .onTrue(
-            new InstantCommand(
-                () -> climber.setTargetPosition(climber.getSettings().minPositionInRads),
-                climberSubsystem));
-
+    // // climber
+    // SubsystemBase climberSubsystem = (SubsystemBase) climber;
+    // // prepare to climb
+    // controller
+    //     .leftBumper()
+    //     .onTrue(
+    //         new InstantCommand(
+    //             () -> climber.setTargetPosition(climber.getSettings().maxPositionInRads),
+    //             climberSubsystem));
+
+    // // climb
+    // controller
+    //     .rightBumper()
+    //     .onTrue(new InstantCommand(() -> climber.setTargetPosition(11.6), climberSubsystem));
+
+    controller.leftBumper().and(ableToIntake).and(scoreMode.negate()).onTrue(intakeCoralCommand);
     // multi controll not workking in each subsystem inde
     controller.povUp().whileTrue(new ElevatorCommand(elevator, () -> 0.2));
 
@@ -163,7 +159,8 @@ public static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm)
             Map.entry(
                 4,
                 new SequentialCommandGroup(
-                        new ElevatorToPosition(elevator, () -> 0.6),
+                        new ElevatorToPosition(elevator, () -> 0.6)
+                            .unless(() -> elevator.getCurrentHeight() > 0.6),
                         new ParallelCommandGroup(
                             new ArmToPosition(coralArm, () -> 48).withTimeout(1.0),
                             new ElevatorToPosition(elevator, () -> 1.553)))
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
index 12dbced3..2138f547 100644
--- a/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
+++ b/src/main/java/frc/robot/subsystems/controls/drive/DriveControls.java
@@ -2,44 +2,42 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.util.FlippingUtil;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.common.arm.ArmToPosition;
 import frc.robot.commands.common.drive.DriveCommand;
+import frc.robot.commands.common.drive.DriveToPositionX;
+import frc.robot.commands.common.drive.DriveToYaw;
+import frc.robot.commands.common.elevator.ElevatorToPosition;
+import frc.robot.subsystems.controls.combination.DriverControls;
+import frc.robot.subsystems.interfaces.Arm;
 import frc.robot.subsystems.interfaces.Drive;
+import frc.robot.subsystems.interfaces.Elevator;
+import frc.robot.util.DevilBotState;
 import java.util.Map;
+import java.util.Map.Entry;
+import org.littletonrobotics.junction.Logger;
 
 public class DriveControls {
-  private enum TargetPoseOption {
-    ORIGIN(0),
-    FEEDER_1(1),
-    FEEDER_2(2),
-    PROCESSOR(3),
-    REEF_A(4),
-    REEF_G(5);
-
-    private int index;
-
-    public int getIndex() {
-      return this.index;
-    }
-
-    // public
-    private TargetPoseOption(int idx) {
-      this.index = idx;
-    }
-  }
 
-  protected static int myCoolPoseKeyIdx = 0;
+  protected static TargetPose chosenTarget = TargetPose.REEF_A;
+  static int value = 0;
 
-  public static void setupController(Drive drive, CommandXboxController controller) {
+  public static void setupController(
+      Drive drive, Elevator elevator, Arm arm, CommandXboxController controller) {
     SubsystemBase driveSubsystem = (SubsystemBase) drive;
     driveSubsystem.setDefaultCommand(
         new DriveCommand(
@@ -61,86 +59,232 @@ public static void setupController(Drive drive, CommandXboxController controller
                     drive.setFieldOrientedDrive(
                         !drive.isFieldOrientedDrive()))); // Toggle Drive Orientation
 
-    // BLUE
-    // // Define destinations for our "dynamic go-to-pose" functionality
-    // Pose2d poseOrigin = new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-    //     poseFeeder1 = new Pose2d(1.05, 7, Rotation2d.fromDegrees(130)),
-    //     poseFeeder2 = new Pose2d(1.05, 1, Rotation2d.fromDegrees(230)),
-    //     poseProcessor = new Pose2d(6, 0.75, Rotation2d.fromDegrees(270)),
-    //     poseReefA = new Pose2d(3.25, 4.05, Rotation2d.fromDegrees(0)),
-    //     poseReefG = new Pose2d(5.5, 3.95, Rotation2d.fromDegrees(180));
-
-    // Define destinations for our "dynamic go-to-pose" functionality
-    Pose2d poseOrigin = new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-        poseFeeder1 = new Pose2d(16.02, 6.9, Rotation2d.fromDegrees(50)),
-        poseFeeder2 = new Pose2d(16.02, 1, Rotation2d.fromDegrees(-50)),
-        poseProcessor = new Pose2d(11.5, 7.3, Rotation2d.fromDegrees(90)),
-        poseReefA = new Pose2d(15, 4.175, Rotation2d.fromDegrees(180)),
-        poseReefAClose = new Pose2d(14.5, 4.175, Rotation2d.fromDegrees(180)),
-        poseReefG = new Pose2d(11, 4.175, Rotation2d.fromDegrees(0)),
-        poseReefGClose = new Pose2d(11.5, 4.175, Rotation2d.fromDegrees(0));
-    // PathConstraints constraints = new PathConstraints(4.2672, 9.4664784, 2 * Math.PI, 4 *
-    // Math.PI);
-    // PathConstraints constraints = new PathConstraints(2, 4.5, 2 * Math.PI, 4 * Math.PI);
-    PathConstraints constraints = new PathConstraints(0.5, 4.5, Math.PI / 4, 4 * Math.PI);
+    PathConstraints constraints =
+        new PathConstraints(
+            drive.getMaxLinearSpeed(), 1.5, drive.getMaxAngularSpeed(), Math.PI / 4);
+
+    SmartDashboard.putData("Go Forward 1 Meter", new DriveToPositionX(drive, () -> 1.0));
 
+    SmartDashboard.putData(
+        "Go Forward 1 Meter and Turn 180",
+        new SequentialCommandGroup(
+            new DriveToPositionX(drive, () -> 1.0), new DriveToYaw(drive, () -> 180.0)));
+
+    SmartDashboard.putData(
+        "Go in a Square Path",
+        new SequentialCommandGroup(
+            new DriveToPositionX(drive, () -> 1.0),
+            new WaitCommand(0.5),
+            new DriveToYaw(drive, () -> 90).withTimeout(2),
+            new WaitCommand(0.5),
+            new DriveToPositionX(drive, () -> 1.0),
+            new WaitCommand(0.5),
+            new DriveToYaw(drive, () -> 180).withTimeout(2),
+            new WaitCommand(0.5),
+            new DriveToPositionX(drive, () -> 1.0),
+            new WaitCommand(0.5),
+            new DriveToYaw(drive, () -> -90).withTimeout(2),
+            new WaitCommand(0.5),
+            new DriveToPositionX(drive, () -> 1.0),
+            new WaitCommand(0.5),
+            new DriveToYaw(drive, () -> 0).withTimeout(2)));
     // Temporary UI to allow user to modify destination on-the-fly
-    SendableChooser<TargetPoseOption> chooser = new SendableChooser<>();
-    chooser.setDefaultOption("Origin", TargetPoseOption.ORIGIN);
-    chooser.addOption("Feeder 1", TargetPoseOption.FEEDER_1);
-    chooser.addOption("Feeder 2", TargetPoseOption.FEEDER_2);
-    chooser.addOption("Processor", TargetPoseOption.PROCESSOR);
-    chooser.addOption("Reef A", TargetPoseOption.REEF_A);
-    chooser.addOption("Reef G", TargetPoseOption.REEF_G);
+    SendableChooser<TargetPose> chooser = new SendableChooser<>();
+    // chooser.setDefaultOption("Origin", TargetPose.ORIGIN);
+    // chooser.addOption("Feeder Left", TargetPose.FEEDER_L);
+    // chooser.addOption("Feeder Right", TargetPose.FEEDER_R);
+    // chooser.addOption("Processor", TargetPose.PROCESSOR);
+    chooser.setDefaultOption("Reef A", TargetPose.REEF_A);
+    // chooser.addOption("Reef A", TargetPose.REEF_A);
+    chooser.addOption("Reef B", TargetPose.REEF_B);
+    chooser.addOption("Reef C", TargetPose.REEF_C);
+    chooser.addOption("Reef D", TargetPose.REEF_D);
+    chooser.addOption("Reef E", TargetPose.REEF_E);
+    chooser.addOption("Reef F", TargetPose.REEF_F);
+    chooser.addOption("Reef G", TargetPose.REEF_G);
+    chooser.addOption("Reef H", TargetPose.REEF_H);
+    chooser.addOption("Reef I", TargetPose.REEF_I);
+    chooser.addOption("Reef J", TargetPose.REEF_J);
+    chooser.addOption("Reef K", TargetPose.REEF_K);
+    chooser.addOption("Reef L", TargetPose.REEF_L);
     SmartDashboard.putData("Pose choices", chooser);
 
     // Define behavior for chosing destination of on-the-fly pose
-    SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
+    SmartDashboard.putNumber("Chosen Pose Index", chosenTarget.getIndex());
+    SmartDashboard.putString("Chosen Reef Position", chosenTarget.getShortName());
     chooser.onChange(
-        (chosenPose) -> {
-          myCoolPoseKeyIdx = chosenPose.getIndex();
-          SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
+        (chosenOption) -> {
+          chosenTarget = chosenOption;
+          SmartDashboard.putNumber("Chosen Pose Index", chosenTarget.getIndex());
+          SmartDashboard.putString("Reef Position", chosenTarget.getShortName());
         });
-    // controller
-    //     .x()
-    //     .onTrue(
-    //         new InstantCommand(
-    //             () -> {
-    //               if (++myCoolPoseKeyIdx == TargetPoseOption.values().length) myCoolPoseKeyIdx =
-    // 1;
-    //               SmartDashboard.putNumber("Chosen Pose Index", myCoolPoseKeyIdx);
-    //             }));
 
     // Define command to go to specific pose
     Command coolGoToPose =
         new SelectCommand<>(
             Map.ofEntries(
-                Map.entry(
-                    TargetPoseOption.ORIGIN.getIndex(),
-                    AutoBuilder.pathfindToPose(poseOrigin, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.FEEDER_1.getIndex(),
-                    AutoBuilder.pathfindToPose(poseFeeder1, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.FEEDER_2.getIndex(),
-                    AutoBuilder.pathfindToPose(poseFeeder2, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.PROCESSOR.getIndex(),
-                    AutoBuilder.pathfindToPose(poseProcessor, constraints, 0.0)),
-                Map.entry(
-                    TargetPoseOption.REEF_A.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefA, constraints, 0.5)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefAClose, constraints, 0.0))),
-                Map.entry(
-                    TargetPoseOption.REEF_G.getIndex(),
-                    AutoBuilder.pathfindToPose(poseReefG, constraints, 0.5)
-                        .andThen(AutoBuilder.pathfindToPose(poseReefGClose, constraints, 0.0)))),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_A,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_B,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_C,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_D,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_E,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_F,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_G,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_H,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_I,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_J,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_K,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0)),
+                coolDynamicPathScoringCommand(
+                    TargetPose.REEF_L,
+                    constraints,
+                    getPrepareToScoreCommand(elevator, arm),
+                    new ArmToPosition(arm, () -> 0))),
             () -> {
-              return myCoolPoseKeyIdx;
+              return chosenTarget.getIndex();
             });
-    // ), () -> { return chooser.getSelected(); });
 
     // dynamically go to destination
-    controller.b().whileTrue(coolGoToPose);
+    controller.rightTrigger().whileTrue(coolGoToPose);
+
+    /*  Angles -> Reef positions
+     * 0    30  : E
+     * 30   60  : F
+     * 60   90  : G
+     * 90   120 : H
+     * 120  150 : I
+     * 150  180 : J
+     * 180  210 : K
+     * 210  240 : L
+     * 240  270 : A
+     * 270  300 : B
+     * 300  330 : C
+     * 330  360 : D
+     */
+  }
+
+  public static void setupAssistantController(Drive drive, CommandXboxController controller) {
+    controller
+        .rightTrigger()
+        .whileTrue(
+            new RunCommand(
+                () -> {
+                  double myX = controller.getLeftX();
+                  double myY = -controller.getLeftY();
+
+                  // TODO maybe keep calculation in radians?
+                  double myNumber = Math.atan2(myY, myX) * (180 / Math.PI);
+                  if (myNumber < 0) myNumber += 360; // obtain this angle as a positive number
+
+                  // Convert the joystick angle to a reef position
+                  chosenTarget = TargetPose.getReefTargetWithAngle(myNumber);
+                  SmartDashboard.putNumber("Chosen Pose Index", chosenTarget.getIndex());
+                  SmartDashboard.putString("Chosen Reef Position", chosenTarget.getShortName());
+
+                  // Log target pose (for debugging)
+                  Pose2d p =
+                      DevilBotState.isRedAlliance()
+                          ? chosenTarget.getPose()
+                          : FlippingUtil.flipFieldPose(chosenTarget.getPose());
+                  Logger.recordOutput("DriveSwerveYAGSL/Chosen Reef Pose", p);
+                  SmartDashboard.putNumber("Chosen Reef X", p.getX());
+                  SmartDashboard.putNumber("Chosen Reef Y", p.getY());
+                  SmartDashboard.putNumber("Chosen Reef Rot", p.getRotation().getDegrees());
+                }));
+  }
+
+  private static Entry<Integer, Command> coolDynamicPathScoringCommand(
+      TargetPose target,
+      PathConstraints constraints,
+      Command prepareToScoreCommand,
+      Command scoreCommand) {
+    return Map.entry(
+        target.getIndex(),
+        new SequentialCommandGroup(
+            AutoBuilder.pathfindToPoseFlipped(target.getPrepPose(), constraints, 0.0),
+            prepareToScoreCommand,
+            new ConditionalCommand(
+                AutoBuilder.pathfindToPoseFlipped(target.getPose(), constraints, 0.0),
+                AutoBuilder.pathfindToPoseFlipped(target.getLowerScoringPose(), constraints, 0.0),
+                () -> DriverControls.Constants.prepareScoreSelctedIndex < 4),
+            scoreCommand));
+  }
+
+  private static Command getPrepareToScoreCommand(Elevator elevator, Arm coralArm) {
+    return new SelectCommand<>(
+        Map.ofEntries(
+            Map.entry(
+                1,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 0.63),
+                        new ArmToPosition(coralArm, () -> 47))
+                    .withTimeout(5.0)),
+            Map.entry(
+                2,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 0.63),
+                        new ArmToPosition(coralArm, () -> 47))
+                    .withTimeout(5.0)),
+            Map.entry(
+                3,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 1.0),
+                        new ArmToPosition(coralArm, () -> 47))
+                    .withTimeout(5.0)),
+            Map.entry(
+                4,
+                new SequentialCommandGroup(
+                        new ElevatorToPosition(elevator, () -> 0.6)
+                            .unless(() -> elevator.getCurrentHeight() > 0.6),
+                        new ParallelCommandGroup(
+                            new ArmToPosition(coralArm, () -> 48).withTimeout(1.0),
+                            new ElevatorToPosition(elevator, () -> 1.553)))
+                    .withTimeout(5.0))),
+        () -> {
+          return DriverControls.Constants.prepareScoreSelctedIndex;
+        });
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
new file mode 100644
index 00000000..6a071ddb
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/controls/drive/TargetPose.java
@@ -0,0 +1,191 @@
+package frc.robot.subsystems.controls.drive;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+
+public enum TargetPose {
+  // NOTE: WE ARE IDENTIFYING THESE POSITIONS ON THE BLUE SIDE OF THE FIELD!!!
+  // THEN, DEPENDING ON THE ALLIANCE GIVEN TO THE DRIVER STATION,
+  // PATHPLANNER WILL DECIDE WHETHER THE DESTINATION POSE IS
+  // ON THE RED OR BLUE SIDE OF THE FIELD
+
+  ORIGIN(0, "O", "Origin", new Pose2d(0.0, 0.0, new Rotation2d(0))),
+  FEEDER_R(1, "FR", "Feeder Right", new Pose2d(1.05, 1, new Rotation2d(50))),
+  FEEDER_L(2, "FL", "Feeder Left", new Pose2d(1.05, 7, Rotation2d.fromDegrees(-50))),
+  REEF_A(3, "A", "Reef A", 0.0, true),
+  REEF_B(4, "B", "Reef B", 0.0, false),
+  REEF_C(5, "C", "Reef C", 60.0, true),
+  REEF_D(6, "D", "Reef D", 60.0, false),
+  REEF_E(7, "E", "Reef E", 120.0, true),
+  REEF_F(8, "F", "Reef F", 120.0, false),
+  REEF_G(9, "G", "Reef G", 180.0, true),
+  REEF_H(10, "H", "Reef H", 180.0, false),
+  REEF_I(11, "I", "Reef I", 240.0, true),
+  REEF_J(12, "J", "Reef J", 240.0, false),
+  REEF_K(13, "K", "Reef K", 300.0, true),
+  REEF_L(14, "L", "Reef L", 300.0, false),
+  PROCESSOR(15, "P", "Processor", new Pose2d(6, 0.75, new Rotation2d(270)));
+
+  // REMEMBER: actual distance between reef poles is 13 inches,
+  // currently using 9 inches because robot currently overshoots target pose during traversal
+
+  private final Pose2d blueReefCenter =
+      new Pose2d(new Translation2d(4.485, 4.00), new Rotation2d());
+
+  // NOTE: I pushed the target position closer to the reef to be more realistic to the actual game
+  // the original position was x: 3.16, y: 3.82
+  private final Pose2d blueReefBPos = new Pose2d(new Translation2d(3.175, 3.82), new Rotation2d());
+
+  public static TargetPose getReefTargetWithAngle(double angleDegrees) {
+    int reefPosIdx = (int) (angleDegrees / 30) + 7;
+    if (reefPosIdx > 14) reefPosIdx -= 12;
+    for (TargetPose x : values()) {
+      if (x.getIndex() == reefPosIdx) return x;
+    }
+    throw new IllegalArgumentException(reefPosIdx + " does not translate to valid reef position.");
+  }
+
+  /*
+   *  Home positions (blue)
+   * A:
+   * B: 3.16 3.80
+   * C: 3.72 3.03 60
+   * D:
+   * E:
+   * F:
+   * G:
+   * H:
+   * I:
+   * J:
+   * K:
+   * L:
+   *
+   *
+   * camera
+   * x -7
+   * y 2
+   * z 22.5in
+   * 23.75
+   * roll 0
+   * pitch 21
+   * yaw 10
+   */
+
+  /*
+   *  Practice field testing
+   * A: 3.13 4.18 0           14.36 3.88 180
+   * B: 3.13 3.79 0           14.42 4.26 180
+   * C: 3.71 2.98 59.6        13.83 5.07 -120.3
+   * D: 4.10 2.76 60          13.45 5.29 -120
+   * E: 5.00 2.83 120.0       12.55 5.23 -60
+   * F: 5.36 3.00 122.5~      12.19 5.06 -57.7    ** this one potentially uncentered
+   * G: 5.71 4.00 -177.23     11.84 4.07 2.77
+   * H:
+   * I: 5.28 5.06 -119.74     12.26 3.00 60.26
+   * J: 4.97 5.28 -119.62     12.57 2.78 60.38
+   * K: 3.95 5.18 -59         13.59 2.87 121
+   * L: 3.69 5.04 -58.6~      13.86 3.01 121.3
+   */
+
+  /*
+   *  During practice matches
+   * A:
+   * B:
+   * C:
+   * D:
+   * E:
+   * F:
+   * G: 5.55 3.83 178.7       11.99 4.22 -1.30
+   * H: 5.56 4.20 177.95      11.99 3.85 -3.05
+   * I: 5.27 5.03 -125.37     12.28 3.02 54.63    ?
+   * J: 4.71 5.14 -123.69     12.84 2.92 56.31    ??
+   * K:
+   * L? 3.49 4.61 -57.59      14.06 3.44 122.41   ??
+   */
+
+  private int index;
+  private String shortName;
+  private String longName;
+
+  private Pose2d pose;
+  private Pose2d prepPose;
+  private Pose2d lowerLevelPose;
+  private Pose2d endPose;
+
+  // NOTE differential poses ~= 0.5m away from target
+
+  public int getIndex() {
+    return this.index;
+  }
+
+  public String getShortName() {
+    return this.shortName;
+  }
+
+  public String getLongName() {
+    return this.longName;
+  }
+
+  public Pose2d getPose() {
+    return this.pose;
+  }
+
+  public static Pose2d getPosewWithIndex(int index) {
+    for (TargetPose x : values()) {
+      if (x.getIndex() == index) {
+        return x.pose;
+      }
+    }
+    return new Pose2d();
+  }
+
+  public Pose2d getPrepPose() {
+    return this.prepPose;
+  }
+
+  public Pose2d getEndPose(boolean isRed) {
+    return this.endPose;
+  }
+
+  public Pose2d getLowerScoringPose() {
+    return this.lowerLevelPose;
+  }
+
+  private TargetPose(int idx, String sName, String lName, Pose2d targetPose) {
+    this.index = idx;
+    this.shortName = sName;
+    this.longName = lName;
+    this.pose = targetPose;
+
+    // TODO handle error case of accessing prep and end poses on non-reef position
+
+    this.lowerLevelPose = null;
+    this.prepPose = null;
+    this.endPose = null;
+  }
+
+  private TargetPose(
+      int idx, String sName, String lName, double reefWallRotDegrees, boolean isLeftPosition) {
+    this.index = idx;
+    this.shortName = sName;
+    this.longName = lName;
+
+    this.pose =
+        blueReefBPos.rotateAround(
+            blueReefCenter.getTranslation(), Rotation2d.fromDegrees(reefWallRotDegrees));
+    double distance = 0.6;
+    double rot = -Units.degreesToRadians(25.0);
+    if (isLeftPosition) {
+      this.pose =
+          this.pose.transformBy(new Transform2d(0, Units.inchesToMeters(13), new Rotation2d()));
+      distance = -distance;
+      rot = -rot;
+    }
+    this.prepPose = this.pose.transformBy(new Transform2d(-0.2, distance, new Rotation2d(rot)));
+    this.lowerLevelPose = this.pose.transformBy(new Transform2d(0.04445, 0, new Rotation2d()));
+    this.endPose = this.pose.transformBy(new Transform2d(-0.7, 0, new Rotation2d()));
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
index 459c223f..f57fe1b0 100644
--- a/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
+++ b/src/main/java/frc/robot/subsystems/implementations/drive/DriveSwerveYAGSL.java
@@ -11,13 +11,13 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import frc.robot.io.interfaces.DriveIO;
 import frc.robot.io.interfaces.DriveIOInputsAutoLogged;
 import frc.robot.subsystems.interfaces.Drive;
+import frc.robot.util.DevilBotState;
 import java.io.File;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -25,6 +25,7 @@
 import swervelib.SwerveDriveTest;
 import swervelib.parser.SwerveParser;
 import swervelib.telemetry.SwerveDriveTelemetry;
+import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 
 public class DriveSwerveYAGSL extends DriveBase {
   public static class Constants {
@@ -35,11 +36,11 @@ public static class Constants {
 
   private final File swerveJsonDirectory;
   private SwerveDrive swerveDrive;
-  @AutoLogOutput private boolean fieldOrientedDrive = false;
+  @AutoLogOutput private boolean fieldOrientedDrive = true;
   private PIDConstants translationPIDConstants =
-      new PIDConstants(5.0, 0.0, 0.0); // Translation PID constants
+      new PIDConstants(7.5, 0.0, 0.0); // Translation PID constants
   private PIDConstants rotationPIDConstants =
-      new PIDConstants(5.0, 0.0, 0.0); // Rotation PID constants
+      new PIDConstants(6.0, 0.0, 0.0); // Rotation PID constants
 
   // @AutoLogOutput
   DriveIO io = new DriveIO();
@@ -49,7 +50,7 @@ public DriveSwerveYAGSL(String configPath) {
     super("YAGSL");
     swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), configPath);
 
-    // \SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
+    SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
     try {
       swerveDrive =
           new SwerveParser(swerveJsonDirectory)
@@ -113,11 +114,7 @@ private boolean setupPathPlanner(
             // Boolean supplier that controls when the path will be mirrored for the red alliance
             // This will flip the path being followed to the red side of the field.
             // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-            var alliance = DriverStation.getAlliance();
-            if (alliance.isPresent()) {
-              return alliance.get() == DriverStation.Alliance.Red;
-            }
-            return false;
+            return DevilBotState.isRedAlliance();
           },
           this // Reference to this subsystem to set requirements
           );
@@ -313,7 +310,6 @@ public void lockPose() {
   public void periodic() {
     io.updateInputs(inputs, swerveDrive);
     Logger.processInputs("Drive", inputs);
-
     // Because the asynchronous odometry updates have been disabled,
     // we invoke updates manually
     swerveDrive.updateOdometry();
diff --git a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
index beee924f..98b1753c 100644
--- a/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/implementations/vision/VisionSubsystem.java
@@ -9,11 +9,9 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.interfaces.Vision;
 import java.util.ArrayList;
-import java.util.LinkedList;
 import java.util.List;
 import java.util.Optional;
 import java.util.function.Supplier;
-import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
@@ -40,7 +38,7 @@ private class VisionCameraImpl {
     private final int index;
     private PhotonCameraSim simCamera;
     private PhotonPipelineResult result;
-    private Optional<EstimatedRobotPose> estimatedRobotPose;
+    private Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
 
     private VisionCameraImpl(Camera camera, AprilTagFieldLayout fieldLayout) {
       this.camera = new PhotonCamera(camera.getName());
@@ -53,6 +51,7 @@ private VisionCameraImpl(Camera camera, AprilTagFieldLayout fieldLayout) {
     }
 
     private void update() {
+      // This methods is invoked "periodically"
       for (PhotonPipelineResult photonPipelineResult : camera.getAllUnreadResults()) {
         estimatedRobotPose = poseEstimator.update(photonPipelineResult);
         result = photonPipelineResult;
@@ -99,24 +98,13 @@ private int getIndex() {
     private String getName() {
       return camera.getName();
     }
-
-    private Transform3d getRobotToCamera() {
-      return robotToCamera;
-    }
   }
 
   private final List<VisionCameraImpl> cameras = new ArrayList<VisionCameraImpl>();
-  private VisionCameraImpl primaryCamera = null;
   private final AprilTagFieldLayout fieldLayout;
   private VisionMeasurementConsumer visionMeasurementConsumer = null;
   List<Camera> visionCameras = new ArrayList<Camera>();
 
-  /* Debug Info */
-  @AutoLogOutput private int debugTargetsVisible;
-
-  @AutoLogOutput private double debugTargetDistance = 0;
-  @AutoLogOutput private double debugTargetYaw = 0;
-
   /* Simulation Support*/
   private boolean simEnabled = false;
   private VisionSystemSim simVision;
@@ -130,10 +118,6 @@ public VisionSubsystem(AprilTagFieldLayout fieldLayout) {
   public void addCamera(Camera camera) {
     cameras.add(new VisionCameraImpl(camera, fieldLayout));
     visionCameras.add(camera);
-
-    if (1 == cameras.size()) {
-      primaryCamera = this.cameras.get(0);
-    }
   }
 
   @Override
@@ -163,8 +147,6 @@ public void enableSimulation(Supplier<Pose2d> poseSupplier, boolean enableWireFr
   }
 
   public void updatePoseEstimation() {
-    // Initialize new list of robot poses (for debugging)
-    List<Pose2d> debugRobotPoses = new LinkedList<>();
 
     if (simEnabled) {
       simVision.update(simPoseSupplier.get());
@@ -173,36 +155,35 @@ public void updatePoseEstimation() {
     for (VisionCameraImpl camera : cameras) {
       camera.update();
 
+      // Default values: in case no targets are seen
+      double distanceToTarget = -1;
+      Pose2d estimatedRobotPoseFromCamera = new Pose2d(-10.0, -10.0, new Rotation2d(0));
+
       Optional<EstimatedRobotPose> currentEstimatedRobotPose = camera.getEstimatedRobotPose();
       if (currentEstimatedRobotPose.isPresent()) {
-        double distanceToBestTarget = camera.getDistanceToBestTarget();
+
+        estimatedRobotPoseFromCamera = currentEstimatedRobotPose.get().estimatedPose.toPose2d();
+        distanceToTarget = camera.getDistanceToBestTarget();
 
         // Add vision measurement to the consumer.
-        if (visionMeasurementConsumer != null && distanceToBestTarget > 5) {
+        if (visionMeasurementConsumer != null && distanceToTarget < 0.8) {
           visionMeasurementConsumer.add(
-              currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
+              estimatedRobotPoseFromCamera,
               currentEstimatedRobotPose.get().timestampSeconds,
-              VecBuilder.fill(
-                  distanceToBestTarget / 2, distanceToBestTarget / 2, distanceToBestTarget / 2));
+              VecBuilder.fill(distanceToTarget / 2, distanceToTarget / 2, distanceToTarget / 2));
         }
         /* NOTE standard deviation format:
          * (x position in meters, y position in meters, and heading in radians)
          * Increase these numbers to trust the vision pose measurement less.
          */
-
-        // Log estimated robot pose for debugging
-        debugRobotPoses.add(currentEstimatedRobotPose.get().estimatedPose.toPose2d());
-      } else {
-        // Display the robot pose "out of the arena" (indicating no pose found)
-        debugRobotPoses.add(new Pose2d(-10.0, -10.0, new Rotation2d(0)));
       }
-    }
 
-    // Record estimated robot pose to AdvantageKit networktables
-    if (debugRobotPoses.size() > 0) {
+      // Log information for debugging
+      Logger.recordOutput(
+          "VisionSubsystem/Distances To Target/" + camera.getName(), distanceToTarget);
       Logger.recordOutput(
-          "VisionSubsystem/DEBUGestimatedCameraPoses",
-          debugRobotPoses.toArray(new Pose2d[debugRobotPoses.size()]));
+          "VisionSubsystem/Estimated Robot Poses/" + camera.getName(),
+          estimatedRobotPoseFromCamera);
     }
   }
 
@@ -211,82 +192,6 @@ public void periodic() {
     updatePoseEstimation();
   }
 
-  private PhotonTrackedTarget findAprilTag(int id) {
-    if (primaryCamera != null) {
-      for (PhotonTrackedTarget target : primaryCamera.getTargets()) {
-        if (target.getFiducialId() == id) {
-          return target;
-        }
-      }
-    }
-    return null;
-  }
-
-  @Override
-  public Optional<Double> getDistanceToAprilTag(int id) {
-    PhotonTrackedTarget target = findAprilTag(id);
-
-    if ((target != null) && (primaryCamera != null)) {
-      return Optional.of(
-          PhotonUtils.calculateDistanceToTargetMeters(
-                  primaryCamera.getRobotToCamera().getZ(),
-                  fieldLayout.getTagPose(target.getFiducialId()).get().getZ(),
-                  -primaryCamera.getRobotToCamera().getRotation().getY(),
-                  Units.degreesToRadians(target.getPitch()))
-              + Constants.visionDistanceOffsetInMeters);
-    }
-    return Optional.empty();
-  }
-
-  @Override
-  public Optional<Integer> getBestTargetId() {
-    if ((primaryCamera != null) && (primaryCamera.hasTargets())) {
-      return Optional.of(primaryCamera.getBestTarget().getFiducialId());
-    }
-    return Optional.empty();
-  }
-
-  @Override
-  public Optional<Double> getYawToBestTarget() {
-    if ((primaryCamera != null) && (primaryCamera.hasTargets())) {
-      return Optional.of(primaryCamera.getBestTarget().getYaw());
-    }
-    return Optional.empty();
-  }
-
-  @Override
-  public Optional<Double> getDistanceToBestTarget() {
-    if ((primaryCamera != null) && (primaryCamera.hasTargets())) {
-      return getDistanceToAprilTag(primaryCamera.getBestTarget().getFiducialId());
-    }
-    return Optional.empty();
-  }
-
-  @Override
-  public Optional<Double> getYawToAprilTag(int id) {
-    PhotonTrackedTarget target = findAprilTag(id);
-
-    if (target != null) {
-      return Optional.of(target.getYaw());
-    }
-
-    return Optional.empty();
-  }
-
-  @Override
-  public boolean setPrimaryCamera(String name) {
-    boolean foundCamera = false;
-
-    for (VisionCameraImpl camera : cameras) {
-      if (camera.getName().equals(name)) {
-        primaryCamera = camera;
-        foundCamera = true;
-        break;
-      }
-    }
-    return foundCamera;
-  }
-
   @Override
   public void setVisionMeasurementConsumer(VisionMeasurementConsumer func) {
     visionMeasurementConsumer = func;
diff --git a/src/main/java/frc/robot/subsystems/interfaces/Vision.java b/src/main/java/frc/robot/subsystems/interfaces/Vision.java
index 129c2c7e..d60a046d 100644
--- a/src/main/java/frc/robot/subsystems/interfaces/Vision.java
+++ b/src/main/java/frc/robot/subsystems/interfaces/Vision.java
@@ -7,7 +7,6 @@
 import edu.wpi.first.math.numbers.N3;
 import java.text.DecimalFormat;
 import java.util.List;
-import java.util.Optional;
 import java.util.function.Supplier;
 
 public interface Vision {
@@ -108,38 +107,6 @@ public String toString() {
     }
   }
 
-  public default boolean setPrimaryCamera(String name) {
-    return true;
-  }
-
-  public Optional<Integer> getBestTargetId();
-
-  public Optional<Double> getDistanceToBestTarget();
-
-  /**
-   * Returns the yaw in degrees to the best target (relative to the primary camera)
-   *
-   * @return yaw to the best target (in degrees)
-   */
-  public Optional<Double> getYawToBestTarget();
-
-  /**
-   * Returns the distance to the specified april tag in meters (relative to the primary camera)
-   *
-   * @param id AprilTag ID
-   * @return distance to the specified april tag (in meters).
-   */
-  public Optional<Double> getDistanceToAprilTag(int id);
-
-  /**
-   * Returns the yaw in degrees to the specified april tag in meters (relative to the primary
-   * camera)
-   *
-   * @param id AprilTag ID
-   * @return yaw to the specified april tag (in degrees).
-   */
-  public Optional<Double> getYawToAprilTag(int id);
-
   public default void enableSimulation(Supplier<Pose2d> poseSupplier, boolean enableWireFrame) {}
 
   public default void setVisionMeasurementConsumer(VisionMeasurementConsumer func) {}
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 03df051a..bef4a151 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,7 +1,7 @@
 {
     "fileName": "AdvantageKit.json",
     "name": "AdvantageKit",
-    "version": "4.1.0",
+    "version": "4.1.2",
     "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-java",
-            "version": "4.1.0"
+            "version": "4.1.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-wpilibio",
-            "version": "4.1.0",
+            "version": "4.1.2",
             "skipInvalidPlatforms": false,
             "isJar": false,
             "validPlatforms": [
diff --git a/vendordeps/PathplannerLib-2025.2.4.json b/vendordeps/PathplannerLib-2025.2.7.json
similarity index 87%
rename from vendordeps/PathplannerLib-2025.2.4.json
rename to vendordeps/PathplannerLib-2025.2.7.json
index 24add570..d3f84e53 100644
--- a/vendordeps/PathplannerLib-2025.2.4.json
+++ b/vendordeps/PathplannerLib-2025.2.7.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.2.4.json",
+    "fileName": "PathplannerLib-2025.2.7.json",
     "name": "PathplannerLib",
-    "version": "2025.2.4",
+    "version": "2025.2.7",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.2.4"
+            "version": "2025.2.7"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.2.4",
+            "version": "2025.2.7",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/Phoenix6-25.3.1.json b/vendordeps/Phoenix6-frc2025-latest.json
similarity index 86%
rename from vendordeps/Phoenix6-25.3.1.json
rename to vendordeps/Phoenix6-frc2025-latest.json
index a216d975..6f40c840 100644
--- a/vendordeps/Phoenix6-25.3.1.json
+++ b/vendordeps/Phoenix6-frc2025-latest.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.3.1.json",
+    "fileName": "Phoenix6-frc2025-latest.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.3.1",
+    "version": "25.4.0",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.3.1"
+            "version": "25.4.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -208,7 +208,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "25.3.1",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -224,7 +238,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -240,7 +254,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -256,7 +270,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -272,7 +286,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -288,7 +302,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -304,7 +318,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -320,7 +334,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -336,7 +350,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -352,7 +366,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -368,7 +382,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -384,7 +398,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -400,7 +414,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -416,7 +430,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -432,7 +446,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "25.3.1",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANdi",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -444,6 +458,22 @@
                 "osxuniversal"
             ],
             "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProCANdle",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
         }
     ]
 }
\ No newline at end of file
diff --git a/vendordeps/REVLib-2025.0.2.json b/vendordeps/REVLib.json
similarity index 90%
rename from vendordeps/REVLib-2025.0.2.json
rename to vendordeps/REVLib.json
index c29aefa1..ac62be88 100644
--- a/vendordeps/REVLib-2025.0.2.json
+++ b/vendordeps/REVLib.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib-2025.0.2.json",
+    "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2025.0.2",
+    "version": "2025.0.3",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.2"
+            "version": "2025.0.3"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.2",
+            "version": "2025.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -36,7 +36,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.2",
+            "version": "2025.0.3",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -53,7 +53,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.2",
+            "version": "2025.0.3",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json
index 00d3f73f..61a3f5da 100644
--- a/vendordeps/ThriftyLib.json
+++ b/vendordeps/ThriftyLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "ThriftyLib.json",
     "name": "ThriftyLib",
-    "version": "2025.0.2",
+    "version": "2025.1.1",
     "frcYear": "2025",
     "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.thethriftybot.frc",
             "artifactId": "ThriftyLib-java",
-            "version": "2025.0.2"
+            "version": "2025.1.1"
         }
     ],
     "jniDependencies": [],
diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json
index 579c2609..264dc8f5 100644
--- a/vendordeps/maple-sim.json
+++ b/vendordeps/maple-sim.json
@@ -1,7 +1,7 @@
 {
     "fileName": "maple-sim.json",
     "name": "maplesim",
-    "version": "0.3.8",
+    "version": "0.3.13",
     "frcYear": "2025",
     "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.ironmaple",
             "artifactId": "maplesim-java",
-            "version": "0.3.8"
+            "version": "0.3.13"
         },
         {
             "groupId": "org.dyn4j",
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index 6af3d3e2..2d7b1d8e 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "photonlib.json",
     "name": "photonlib",
-    "version": "v2025.1.1",
+    "version": "v2025.3.1",
     "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
     "frcYear": "2025",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-cpp",
-            "version": "v2025.1.1",
+            "version": "v2025.3.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -28,7 +28,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-cpp",
-            "version": "v2025.1.1",
+            "version": "v2025.3.1",
             "libName": "photonlib",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -43,7 +43,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-cpp",
-            "version": "v2025.1.1",
+            "version": "v2025.3.1",
             "libName": "photontargeting",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -60,12 +60,12 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-java",
-            "version": "v2025.1.1"
+            "version": "v2025.3.1"
         },
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-java",
-            "version": "v2025.1.1"
+            "version": "v2025.3.1"
         }
     ]
 }
\ No newline at end of file