After severn

.vscode/settings.json

@@ -58,5 +58,6 @@
     "edu.wpi.first.math.**.struct.*",
   ],
   "java.dependency.enableDependencyCheckup": false,
-  "java.debug.settings.onBuildFailureProceed": true
+  "java.debug.settings.onBuildFailureProceed": true,
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
 }

build.gradle

@@ -74,6 +74,8 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    implementation 'org.apache.commons:commons-math3:3.6.1'
 }
 
 test {

src/main/deploy/pathplanner/autos/center to back outpost side.auto → src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto

@@ -4,32 +4,39 @@
     "type": "sequential",
     "data": {
       "commands": [
-        {
-          "type": "named",
-          "data": {
-            "name": "hopper deploy"
-          }
-        },
         {
           "type": "named",
           "data": {
             "name": "idle"
           }
         },
         {
-          "type": "deadline",
+          "type": "parallel",
           "data": {
             "commands": [
               {
-                "type": "path",
+                "type": "deadline",
                 "data": {
-                  "pathName": "1 Outpost side center"
+                  "commands": [
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "1 Depot side center"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
                 }
               },
               {
                 "type": "named",
                 "data": {
-                  "name": "intake"
+                  "name": "hopper deploy"
                 }
               }
             ]
@@ -38,7 +45,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "2 center to trench outpost"
+            "pathName": "2 Center back depot"
           }
         },
         {
@@ -75,7 +82,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "3 to middle outpost"
+            "pathName": "3 to middle depot"
           }
         }
       ]

src/main/deploy/pathplanner/autos/center to back depot side.auto

@@ -11,25 +11,45 @@
           }
         },
         {
-          "type": "deadline",
+          "type": "parallel",
           "data": {
             "commands": [
               {
-                "type": "path",
+                "type": "deadline",
                 "data": {
-                  "pathName": "1 Depot side center"
+                  "commands": [
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "1 Depot side center"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
                 }
               },
               {
-                "type": "named",
-                "data": {
-                  "name": "hopper deploy"
-                }
-              },
-              {
-                "type": "named",
+                "type": "sequential",
                 "data": {
-                  "name": "intake"
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "hopper deploy"
+                      }
+                    }
+                  ]
                 }
               }
             ]

src/main/deploy/pathplanner/paths/1 Depot side center.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 7.74248120300752,
-        "y": 4.862545112781954
+        "x": 7.753707692307692,
+        "y": 5.793184615384616
       },
       "prevControl": {
-        "x": 7.636977443609023,
-        "y": 8.001281954887219
+        "x": 7.802546153846153,
+        "y": 7.760676923076924
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,21 +34,7 @@
       "rotationDegrees": -58.149754025069136
     }
   ],
-  "constraintZones": [
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 0.6399186643835615,
-      "maxWaypointRelativePos": 1.0,
-      "constraints": {
-        "maxVelocity": 1.5,
-        "maxAcceleration": 1.5,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    }
-  ],
+  "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {

src/main/deploy/pathplanner/paths/2 Center back depot.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.735228859581072,
-        "y": 4.791419705197828
+        "x": 7.649053846153848,
+        "y": 5.96063076923077
       },
       "prevControl": null,
       "nextControl": {
-        "x": 8.002614429790537,
-        "y": 7.880426687354538
+        "x": 7.537423076923078,
+        "y": 7.593230769230768
       },
       "isLocked": false,
       "linkedName": null

src/main/deploy/pathplanner/paths/3 outpost double dip out.path

@@ -0,0 +1,68 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.763007521856398,
+        "y": 0.5450873093377981
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.79049072265625,
+        "y": 0.5080256580171136
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.756997593470982,
+        "y": 3.5701235235305067
+      },
+      "prevControl": {
+        "x": 6.823525041852679,
+        "y": 1.1877271670386897
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.6108732876712327,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 89.52460411845328
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 79.34104871052683
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/4 outpost double dip back.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.581018473307291,
+        "y": 3.4284531947544643
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.592998511904762,
+        "y": 1.1323131161644344
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.681951090494791,
+        "y": 0.5692513020833336
+      },
+      "prevControl": {
+        "x": 6.184096854073661,
+        "y": 0.4140205891927087
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 77.58672595112199
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 89.64828909075314
+  },
+  "useDefaultConstraints": true
+}