Added Amp Side Escape

Author: Jzmoore27

src/main/deploy/pathplanner/paths/6 Shoot.path

@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 4.635439403975525,
+        "x": 4.64,
         "y": 4.625468700804305
       },
       "prevControl": {
-        "x": 5.8263814243891465,
+        "x": 5.830942020413621,
         "y": 4.076031404657965
       },
       "nextControl": null,

src/main/deploy/pathplanner/paths/Amp Side Escape.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.64,
+        "y": 4.625468700804305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.186919968145987,
+        "y": 7.7808159412285605
+      },
+      "isLocked": false,
+      "linkedName": "6 Shoot End"
+    },
+    {
+      "anchor": {
+        "x": 8.26051834784571,
+        "y": 6.651185591813652
+      },
+      "prevControl": {
+        "x": 7.260518347845711,
+        "y": 6.651185591813652
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.25,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "BETWEEN NOTES",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/frc/robot/RobotContainer.java

@@ -806,6 +806,7 @@ private Command aim() {
   private Command farSideSkip() {
     return new SequentialCommandGroup(
         AutoBuilder.followPath(PathPlannerPath.fromPathFile("Far Start to Preload")),
+        aim(),
         fireSequence(),
         AutoBuilder.followPath(PathPlannerPath.fromPathFile("Source Preload to 4")),
         visionIntake(),
@@ -829,7 +830,8 @@ private Command farSideSkip() {
         visionIntake(),
         AutoBuilder.followPath(PathPlannerPath.fromPathFile("6 Shoot")),
         aim(),
-        fireSequence());
+        fireSequence(),
+        AutoBuilder.followPath(PathPlannerPath.fromPathFile("Amp Side Escape")));
   }
 
   // Bypass Skip original route title
@@ -861,7 +863,8 @@ private Command bypassSkip() {
             () -> indexer.isProx1Active() || indexer.isProx2Active()),
         AutoBuilder.followPath(PathPlannerPath.fromPathFile("6 Shoot")),
         aim(),
-        fireSequence());
+        fireSequence(),
+        AutoBuilder.followPath(PathPlannerPath.fromPathFile("Amp Side Escape")));
   }
 
   // Amp Side Skip original route
@@ -896,7 +899,8 @@ private Command ampSideSkip() {
             () -> indexer.isProx1Active() || indexer.isProx2Active()),
         AutoBuilder.followPath(PathPlannerPath.fromPathFile("6 Shoot")),
         aim(),
-        fireSequence());
+        fireSequence(),
+        AutoBuilder.followPath(PathPlannerPath.fromPathFile("Amp Side Escape")));
   }
 
   private Command pathPlannerCommand() {