diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
index 272c65d..f3c7e32 100644
--- a/.pathplanner/settings.json
+++ b/.pathplanner/settings.json
@@ -15,8 +15,8 @@
     "RealAutons",
     "TestAutos"
   ],
-  "defaultMaxVel": 2.2,
-  "defaultMaxAccel": 1.8,
+  "defaultMaxVel": 2.0,
+  "defaultMaxAccel": 1.3,
   "defaultMaxAngVel": 260.0,
   "defaultMaxAngAccel": 300.0,
   "maxModuleSpeed": 4.5
diff --git a/src/main/deploy/pathplanner/autos/LClose2Far1.auto b/src/main/deploy/pathplanner/autos/LClose2Far1.auto
index 0c257ad..6aee67f 100644
--- a/src/main/deploy/pathplanner/autos/LClose2Far1.auto
+++ b/src/main/deploy/pathplanner/autos/LClose2Far1.auto
@@ -54,6 +54,12 @@
             "name": "ShootMediumGroup"
           }
         },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "1Back"
+          }
+        },
         {
           "type": "path",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/MClose2S1.auto b/src/main/deploy/pathplanner/autos/MClose2S1.auto
index 016dd1f..822b5cf 100644
--- a/src/main/deploy/pathplanner/autos/MClose2S1.auto
+++ b/src/main/deploy/pathplanner/autos/MClose2S1.auto
@@ -55,15 +55,15 @@
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Point2-1"
+            "name": "FloorPickupPosition"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "FloorPickupPosition"
+            "pathName": "Point2-1"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/MClose2S3.auto b/src/main/deploy/pathplanner/autos/MClose2S3.auto
index 080a037..3b2d341 100644
--- a/src/main/deploy/pathplanner/autos/MClose2S3.auto
+++ b/src/main/deploy/pathplanner/autos/MClose2S3.auto
@@ -55,15 +55,15 @@
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Point2-3"
+            "name": "FloorPickupPosition"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "FloorPickupPosition"
+            "pathName": "Point2-3"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/MClose3S13.auto b/src/main/deploy/pathplanner/autos/MClose3S13.auto
index ea99b0c..0c3e31e 100644
--- a/src/main/deploy/pathplanner/autos/MClose3S13.auto
+++ b/src/main/deploy/pathplanner/autos/MClose3S13.auto
@@ -55,15 +55,15 @@
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Point2-1"
+            "name": "FloorPickupPosition"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "FloorPickupPosition"
+            "pathName": "Point2-1"
           }
         },
         {
@@ -97,16 +97,22 @@
             "name": "ShootMediumGroup"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "FloorPickupPosition"
+          }
+        },
         {
           "type": "path",
           "data": {
-            "pathName": "Point1-3"
+            "pathName": "1Back"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "FloorPickupPosition"
+            "pathName": "Point1-3"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/MClose3S31.auto b/src/main/deploy/pathplanner/autos/MClose3S31.auto
index 076a91b..00b1814 100644
--- a/src/main/deploy/pathplanner/autos/MClose3S31.auto
+++ b/src/main/deploy/pathplanner/autos/MClose3S31.auto
@@ -55,15 +55,15 @@
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Point2-3"
+            "name": "FloorPickupPosition"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "FloorPickupPosition"
+            "pathName": "Point2-3"
           }
         },
         {
@@ -97,16 +97,22 @@
             "name": "ShootMediumGroup"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "FloorPickupPosition"
+          }
+        },
         {
           "type": "path",
           "data": {
-            "pathName": "Point3-1"
+            "pathName": "3Back"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "FloorPickupPosition"
+            "pathName": "Point3-1"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/MClose3S31Far1.auto b/src/main/deploy/pathplanner/autos/MClose3S31Far1.auto
index b9d5768..fb0ab31 100644
--- a/src/main/deploy/pathplanner/autos/MClose3S31Far1.auto
+++ b/src/main/deploy/pathplanner/autos/MClose3S31Far1.auto
@@ -97,6 +97,12 @@
             "name": "ShootMediumGroup"
           }
         },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "3Back"
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -140,6 +146,12 @@
             "name": "ShootMediumGroup"
           }
         },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "1Back"
+          }
+        },
         {
           "type": "path",
           "data": {
diff --git a/src/main/deploy/pathplanner/paths/1Back.path b/src/main/deploy/pathplanner/paths/1Back.path
new file mode 100644
index 0000000..bf29a70
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/1Back.path
@@ -0,0 +1,76 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.9042256991849056,
+        "y": 6.461989037114649
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9481582241497215,
+        "y": 6.485862311925948
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.23,
+        "y": 6.63
+      },
+      "prevControl": {
+        "x": 2.186067475035184,
+        "y": 6.606126725188701
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "New Event Marker",
+      "waypointRelativePos": 0,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "ShootMediumPosition"
+              }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "PreRunShooter"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
+    "maxAngularVelocity": 260.0,
+    "maxAngularAcceleration": 300.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 28.52,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "PickupPaths",
+  "previewStartingState": {
+    "rotation": 28.52,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/1Meter.path b/src/main/deploy/pathplanner/paths/1Meter.path
index 0d080a5..86cef1a 100644
--- a/src/main/deploy/pathplanner/paths/1Meter.path
+++ b/src/main/deploy/pathplanner/paths/1Meter.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/1Pickup.path b/src/main/deploy/pathplanner/paths/1Pickup.path
index 6ab2c9a..e8e7120 100644
--- a/src/main/deploy/pathplanner/paths/1Pickup.path
+++ b/src/main/deploy/pathplanner/paths/1Pickup.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/1Reverse.path b/src/main/deploy/pathplanner/paths/1Reverse.path
index 56ec163..afcdf9c 100644
--- a/src/main/deploy/pathplanner/paths/1Reverse.path
+++ b/src/main/deploy/pathplanner/paths/1Reverse.path
@@ -56,8 +56,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/1toF1.path b/src/main/deploy/pathplanner/paths/1toF1.path
index 159169e..aacb430 100644
--- a/src/main/deploy/pathplanner/paths/1toF1.path
+++ b/src/main/deploy/pathplanner/paths/1toF1.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/2Pickup.path b/src/main/deploy/pathplanner/paths/2Pickup.path
index e9ed428..fda1638 100644
--- a/src/main/deploy/pathplanner/paths/2Pickup.path
+++ b/src/main/deploy/pathplanner/paths/2Pickup.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/2Reverse.path b/src/main/deploy/pathplanner/paths/2Reverse.path
index 5b74c9b..2996672 100644
--- a/src/main/deploy/pathplanner/paths/2Reverse.path
+++ b/src/main/deploy/pathplanner/paths/2Reverse.path
@@ -68,8 +68,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/3Back.path b/src/main/deploy/pathplanner/paths/3Back.path
new file mode 100644
index 0000000..1b9db06
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/3Back.path
@@ -0,0 +1,88 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.8732108159384147,
+        "y": 4.632584608774542
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7887022936128545,
+        "y": 4.667364753615702
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.23,
+        "y": 4.46
+      },
+      "prevControl": {
+        "x": 2.3084664747991424,
+        "y": 4.427706507183076
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "New Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 0.1,
+        "maxAngularAcceleration": 0.1
+      }
+    }
+  ],
+  "eventMarkers": [
+    {
+      "name": "New Event Marker",
+      "waypointRelativePos": 0,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "ShootMediumPosition"
+              }
+            },
+            {
+              "type": "named",
+              "data": {
+                "name": "PreRunShooter"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
+    "maxAngularVelocity": 260.0,
+    "maxAngularAcceleration": 300.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -28.519999999999982,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "PickupPaths",
+  "previewStartingState": {
+    "rotation": -28.519999999999982,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/3Pickup.path b/src/main/deploy/pathplanner/paths/3Pickup.path
index bbf180f..fc27707 100644
--- a/src/main/deploy/pathplanner/paths/3Pickup.path
+++ b/src/main/deploy/pathplanner/paths/3Pickup.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/3Reverse.path b/src/main/deploy/pathplanner/paths/3Reverse.path
index 8ee4d25..4183725 100644
--- a/src/main/deploy/pathplanner/paths/3Reverse.path
+++ b/src/main/deploy/pathplanner/paths/3Reverse.path
@@ -68,8 +68,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/ALFirst.path b/src/main/deploy/pathplanner/paths/ALFirst.path
index 45339a4..59cdac2 100644
--- a/src/main/deploy/pathplanner/paths/ALFirst.path
+++ b/src/main/deploy/pathplanner/paths/ALFirst.path
@@ -44,8 +44,8 @@
   ],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/AMFirst.path b/src/main/deploy/pathplanner/paths/AMFirst.path
index c8a3212..dfa1b6d 100644
--- a/src/main/deploy/pathplanner/paths/AMFirst.path
+++ b/src/main/deploy/pathplanner/paths/AMFirst.path
@@ -44,8 +44,8 @@
   ],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/ARFirst.path b/src/main/deploy/pathplanner/paths/ARFirst.path
index 891b49c..331c7f5 100644
--- a/src/main/deploy/pathplanner/paths/ARFirst.path
+++ b/src/main/deploy/pathplanner/paths/ARFirst.path
@@ -44,8 +44,8 @@
   ],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/ARMidway.path b/src/main/deploy/pathplanner/paths/ARMidway.path
index 98efb57..083a20a 100644
--- a/src/main/deploy/pathplanner/paths/ARMidway.path
+++ b/src/main/deploy/pathplanner/paths/ARMidway.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/ARSecond.path b/src/main/deploy/pathplanner/paths/ARSecond.path
index 3dfca12..c97cd4c 100644
--- a/src/main/deploy/pathplanner/paths/ARSecond.path
+++ b/src/main/deploy/pathplanner/paths/ARSecond.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/BlueShoot1.path b/src/main/deploy/pathplanner/paths/BlueShoot1.path
index 4344f8c..494f404 100644
--- a/src/main/deploy/pathplanner/paths/BlueShoot1.path
+++ b/src/main/deploy/pathplanner/paths/BlueShoot1.path
@@ -62,8 +62,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/LFirst.path b/src/main/deploy/pathplanner/paths/LFirst.path
index 1466041..5b56901 100644
--- a/src/main/deploy/pathplanner/paths/LFirst.path
+++ b/src/main/deploy/pathplanner/paths/LFirst.path
@@ -62,8 +62,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/MFirst.path b/src/main/deploy/pathplanner/paths/MFirst.path
index 22d50bf..5376bbb 100644
--- a/src/main/deploy/pathplanner/paths/MFirst.path
+++ b/src/main/deploy/pathplanner/paths/MFirst.path
@@ -62,8 +62,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/Measuring.path b/src/main/deploy/pathplanner/paths/Measuring.path
index 47d5859..47ac3dc 100644
--- a/src/main/deploy/pathplanner/paths/Measuring.path
+++ b/src/main/deploy/pathplanner/paths/Measuring.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/MoveForward.path b/src/main/deploy/pathplanner/paths/MoveForward.path
index ad3a80a..f2038cc 100644
--- a/src/main/deploy/pathplanner/paths/MoveForward.path
+++ b/src/main/deploy/pathplanner/paths/MoveForward.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/Point1-2.path b/src/main/deploy/pathplanner/paths/Point1-2.path
index cb4d332..4cb8ef6 100644
--- a/src/main/deploy/pathplanner/paths/Point1-2.path
+++ b/src/main/deploy/pathplanner/paths/Point1-2.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/Point1-3.path b/src/main/deploy/pathplanner/paths/Point1-3.path
index 8e3c040..967832d 100644
--- a/src/main/deploy/pathplanner/paths/Point1-3.path
+++ b/src/main/deploy/pathplanner/paths/Point1-3.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/Point2-1.path b/src/main/deploy/pathplanner/paths/Point2-1.path
index 58e9fd3..41aca07 100644
--- a/src/main/deploy/pathplanner/paths/Point2-1.path
+++ b/src/main/deploy/pathplanner/paths/Point2-1.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/Point2-3.path b/src/main/deploy/pathplanner/paths/Point2-3.path
index 0ac89b2..6ad690e 100644
--- a/src/main/deploy/pathplanner/paths/Point2-3.path
+++ b/src/main/deploy/pathplanner/paths/Point2-3.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/Point3-1.path b/src/main/deploy/pathplanner/paths/Point3-1.path
index db0f91f..cd760f1 100644
--- a/src/main/deploy/pathplanner/paths/Point3-1.path
+++ b/src/main/deploy/pathplanner/paths/Point3-1.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/Point3-2.path b/src/main/deploy/pathplanner/paths/Point3-2.path
index bad34b9..8e1953e 100644
--- a/src/main/deploy/pathplanner/paths/Point3-2.path
+++ b/src/main/deploy/pathplanner/paths/Point3-2.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/PointF1-F2.path b/src/main/deploy/pathplanner/paths/PointF1-F2.path
index 0f3f88a..5f9b07a 100644
--- a/src/main/deploy/pathplanner/paths/PointF1-F2.path
+++ b/src/main/deploy/pathplanner/paths/PointF1-F2.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/PointF2-F1.path b/src/main/deploy/pathplanner/paths/PointF2-F1.path
index b668ebd..1b32fa5 100644
--- a/src/main/deploy/pathplanner/paths/PointF2-F1.path
+++ b/src/main/deploy/pathplanner/paths/PointF2-F1.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/RFirst.path b/src/main/deploy/pathplanner/paths/RFirst.path
index 0f18a37..1a154ae 100644
--- a/src/main/deploy/pathplanner/paths/RFirst.path
+++ b/src/main/deploy/pathplanner/paths/RFirst.path
@@ -62,8 +62,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/ROut.path b/src/main/deploy/pathplanner/paths/ROut.path
index 76b20ea..583da9a 100644
--- a/src/main/deploy/pathplanner/paths/ROut.path
+++ b/src/main/deploy/pathplanner/paths/ROut.path
@@ -44,8 +44,8 @@
   ],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/Rotate180.path b/src/main/deploy/pathplanner/paths/Rotate180.path
index e5f61c0..1492756 100644
--- a/src/main/deploy/pathplanner/paths/Rotate180.path
+++ b/src/main/deploy/pathplanner/paths/Rotate180.path
@@ -44,8 +44,8 @@
   ],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/SpinTest.path b/src/main/deploy/pathplanner/paths/SpinTest.path
index 2fe7ec2..07f1189 100644
--- a/src/main/deploy/pathplanner/paths/SpinTest.path
+++ b/src/main/deploy/pathplanner/paths/SpinTest.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/TestFromBack.path b/src/main/deploy/pathplanner/paths/TestFromBack.path
index 31eed8a..ec8e27f 100644
--- a/src/main/deploy/pathplanner/paths/TestFromBack.path
+++ b/src/main/deploy/pathplanner/paths/TestFromBack.path
@@ -43,8 +43,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/deploy/pathplanner/paths/ToSpeakerTEst.path b/src/main/deploy/pathplanner/paths/ToSpeakerTEst.path
index b6e570b..85086c7 100644
--- a/src/main/deploy/pathplanner/paths/ToSpeakerTEst.path
+++ b/src/main/deploy/pathplanner/paths/ToSpeakerTEst.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.2,
-    "maxAcceleration": 1.8,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.3,
     "maxAngularVelocity": 260.0,
     "maxAngularAcceleration": 300.0
   },
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 3d27c1e..33381ab 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -121,7 +121,7 @@ public static final class ArmConstants {
 
     public static final double armConversion = 0.05;
 
-    public static final double kMaxOutput = 0.3;
+    public static final double kMaxOutput = 0.4;
 
     // // Arm PID constants
     public static final double kP = 0.65;
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index afb80da..ce4eb5f 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -13,6 +13,7 @@
 package frc.robot;
 
 import com.pathplanner.lib.commands.FollowPathCommand;
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.Constants.EnvironmentalConstants;
@@ -32,6 +33,7 @@
 public class Robot extends LoggedRobot {
   private Command autonomousCommand;
   private RobotContainer robotContainer;
+  Thread m_visionThread;
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -40,6 +42,8 @@ public class Robot extends LoggedRobot {
   @Override
   public void robotInit() {
     // Record metadata
+    CameraServer.startAutomaticCapture();
+
     Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
     Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
     Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b33058c..d2ba6cc 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -38,7 +38,7 @@
 import frc.robot.commands.Positions.StowPosition;
 import frc.robot.commands.PreRunShooter;
 import frc.robot.commands.ShootNote;
-import frc.robot.subsystems.LedSubsystem;
+import frc.robot.commands.ShootNoteTeleop;
 import frc.robot.subsystems.arm.Arm;
 import frc.robot.subsystems.arm.ArmIO;
 import frc.robot.subsystems.arm.ArmIOSim;
@@ -83,7 +83,6 @@ public class RobotContainer {
   private final Intake sIntake;
   private final Arm sArm;
   private final Climber sClimber;
-  private final LedSubsystem ledSubsystem;
   private AprilTagVision aprilTagVision;
 
   // Controllers
@@ -156,8 +155,6 @@ public RobotContainer() {
         break;
     }
 
-    ledSubsystem = new LedSubsystem(0, 80, sIntake);
-
     aprilTagVision.setDataInterfaces(sDrive::addVisionData);
 
     // SmartDashboard.putData(CommandScheduler.getInstance()); //! Command check (testing)
@@ -363,9 +360,7 @@ private void configureButtonBindings() {
                 () -> -driverController.getRightX(),
                 true));
 
-    driverController
-        .start()
-        .whileTrue(Commands.startEnd(() -> sDrive.resetRotation(), sDrive::stop, sDrive));
+    driverController.start().onTrue(Commands.runOnce(() -> sDrive.resetRotation()));
 
     operatorController
         .start()
@@ -398,6 +393,10 @@ private void configureButtonBindings() {
         IntakeShooterControls.intakeShooterDefaultCommand(
             sIntake, () -> operatorController.getRightTriggerAxis()));
 
+    operatorController
+        .start()
+        .whileTrue(Commands.startEnd(() -> sIntake.runFull(), sIntake::stop, sIntake));
+
     operatorController
         .y()
         .whileTrue(Commands.startEnd(() -> sIntake.runPercentSpeed(-1), sIntake::stop, sIntake));
@@ -406,11 +405,12 @@ private void configureButtonBindings() {
     // operatorController.leftTrigger().whileTrue(new IntakeNote(sIntake));
 
     // Flywheel commands
-    operatorController.rightBumper().whileTrue(new ShootNote(sIntake, sFlywheel, sArm));
-    operatorController.leftBumper().whileTrue(new PreRunShooter(sFlywheel, sIntake));
     sFlywheel.setDefaultCommand(
         new PreRunShooter(sFlywheel, true, sIntake)); // Runs the flywheel slowly at all times
 
+    operatorController.rightBumper().whileTrue(new ShootNoteTeleop(sIntake, sFlywheel, sArm));
+    operatorController.leftBumper().whileTrue(new PreRunShooter(sFlywheel, sIntake));
+
     // Climber controls (The first one is 90% probably the one that works.)
     // sClimber.setDefaultCommand(
     //     ClimberCommands.buttonDrive(
diff --git a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java
index d216448..83dde99 100644
--- a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java
+++ b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java
@@ -12,6 +12,7 @@
 
 package frc.robot.commands.ControlCommands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants;
@@ -30,6 +31,8 @@ public static Command intakeShooterDefaultCommand(
             intake.runPercentSpeed(
                 Constants.IntakeConstants.percentPower * rightTriggerSupplier.getAsDouble());
           }
+          boolean noteDetected = intake.getConveyerProximity() || intake.getShooterProximity();
+          SmartDashboard.putBoolean("Note Detected", noteDetected);
         },
         intake);
   }
diff --git a/src/main/java/frc/robot/commands/Positions/FloorPickup.java b/src/main/java/frc/robot/commands/Positions/FloorPickup.java
index 03bc408..ae5bc02 100644
--- a/src/main/java/frc/robot/commands/Positions/FloorPickup.java
+++ b/src/main/java/frc/robot/commands/Positions/FloorPickup.java
@@ -23,7 +23,7 @@ public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ArmAngle = 0.09;
+              double ArmAngle = 0.122;
 
               arm.runTargetAngle(Optional.of(ArmAngle));
             },
diff --git a/src/main/java/frc/robot/commands/Positions/ShootClose.java b/src/main/java/frc/robot/commands/Positions/ShootClose.java
index 93b4737..7d15c53 100644
--- a/src/main/java/frc/robot/commands/Positions/ShootClose.java
+++ b/src/main/java/frc/robot/commands/Positions/ShootClose.java
@@ -23,7 +23,7 @@ public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ArmAngle = 0.747;
+              double ArmAngle = 0.8552;
 
               arm.runTargetAngle(Optional.of(ArmAngle));
             },
diff --git a/src/main/java/frc/robot/commands/Positions/ShootMedium.java b/src/main/java/frc/robot/commands/Positions/ShootMedium.java
index 394e839..0977787 100644
--- a/src/main/java/frc/robot/commands/Positions/ShootMedium.java
+++ b/src/main/java/frc/robot/commands/Positions/ShootMedium.java
@@ -23,7 +23,7 @@ public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ArmAngle = 0.933;
+              double ArmAngle = 0.9783;
 
               arm.runTargetAngle(Optional.of(ArmAngle));
             },
diff --git a/src/main/java/frc/robot/commands/ShootNote.java b/src/main/java/frc/robot/commands/ShootNote.java
index f5929d9..d19635e 100644
--- a/src/main/java/frc/robot/commands/ShootNote.java
+++ b/src/main/java/frc/robot/commands/ShootNote.java
@@ -46,6 +46,7 @@ public void execute() {
     if (arm.getAngleRad() > 1.3) {
       targetRPM = Constants.FlywheelConstants.shootingVelocity / AmpReductionFactor;
     }
+
     flywheel.runVelocity(targetRPM);
 
     if (Math.abs(topFlywheelRPM) > Math.abs(targetRPM * 0.9)) {
diff --git a/src/main/java/frc/robot/commands/ShootNoteTeleop.java b/src/main/java/frc/robot/commands/ShootNoteTeleop.java
new file mode 100644
index 0000000..e41e578
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ShootNoteTeleop.java
@@ -0,0 +1,62 @@
+// Copyright 2016-2024 FRC 5829
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.flywheel.Flywheel;
+import frc.robot.subsystems.intake.Intake;
+
+public class ShootNoteTeleop extends Command {
+
+  private Intake intake;
+  private Flywheel flywheel;
+  private double AmpReductionFactor = 1.5; // amount to lower speed when doing amp
+  private Arm arm;
+
+  public ShootNoteTeleop(Intake intake, Flywheel flywheel, Arm arm) {
+    this.intake = intake;
+    this.flywheel = flywheel;
+    this.arm = arm;
+    addRequirements(intake, flywheel);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    double topFlywheelRPM = -flywheel.getVelocityRPMBottom();
+    double targetRPM = Constants.FlywheelConstants.shootingVelocity;
+
+    if (arm.getAngleRad() > 1.3) {
+      targetRPM = Constants.FlywheelConstants.shootingVelocity / AmpReductionFactor;
+    }
+
+    flywheel.runVelocity(targetRPM);
+
+    if (Math.abs(topFlywheelRPM) > Math.abs(targetRPM * 0.9)) {
+      intake.runPercentSpeed(-1);
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    flywheel.runVelocity(0);
+    intake.runPercentSpeed(0);
+
+    // ShootNoteTeleop ShootNoteTeleopCommand = new ShootNoteTeleop(intake, flywheel, arm);
+    // ShootNoteTeleopCommand.withTimeout(0.5).schedule();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/LedSubsystem.java b/src/main/java/frc/robot/subsystems/LedSubsystem.java
index 1c7d977..4b5dd5c 100644
--- a/src/main/java/frc/robot/subsystems/LedSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/LedSubsystem.java
@@ -9,13 +9,14 @@
 // but WITHOUT ANY WARRANTY; without even the implied warranty of
 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 // GNU General Public License for more details.
-
+/*
 package frc.robot.subsystems;
 
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.intake.Intake;
@@ -30,6 +31,7 @@ public class LedSubsystem extends SubsystemBase {
   private static final double LED_SPEED = 1;
   private final int stripLength;
 
+
   Intake intake;
   private int[] defaultColor;
   private boolean isLucky;
@@ -43,15 +45,9 @@ public LedSubsystem(int LEDPort, int length, Intake intake) {
 
     isLucky = Math.random() > 0.99999;
 
-    try {
-      m_led = new AddressableLED(LEDPort);
-      m_ledBuffer = new AddressableLEDBuffer(length);
-      m_led.setLength(m_ledBuffer.getLength());
-      m_led.setData(m_ledBuffer);
-      m_led.start();
-    } catch (Exception e) {
-      e.printStackTrace();
-    }
+
+
+
   }
 
   public void setLed(int i, int[] color) {
@@ -92,3 +88,4 @@ public void periodic() {
     m_led.setData(m_ledBuffer);
   }
 }
+*/
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java
index a234c5d..f4a1991 100644
--- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java
@@ -43,7 +43,7 @@ public class ArmIOSparkMax implements ArmIO {
 
   private final PIDController mathPid;
 
-  private double targetAngle = 0.747; // 2.2// Radians, just a default value
+  private double targetAngle = 0.09; // 2.2// Radians, just a default value
 
   private double lastEncoderReading = 0.4;
 
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index afb8a69..35ebeb3 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -118,11 +118,11 @@ public Drive(
         () -> kinematics.toChassisSpeeds(getModuleStates()),
         this::runVelocity,
         new HolonomicPathFollowerConfig(
-            new PIDConstants(4.0, 0.1, 0.4),
-            new PIDConstants(10.0, 0.1, 0.0),
+            new PIDConstants(7.0, 0, 0),
+            new PIDConstants(7.0, 0, 0.0),
             CurrentMaxLinearSpeed,
             DRIVE_BASE_RADIUS,
-            new ReplanningConfig()),
+            new ReplanningConfig(false, true)),
         () ->
             DriverStation.getAlliance().isPresent()
                 && DriverStation.getAlliance().get() == Alliance.Red,