Merge branch 'DenverAutos' into New-Intake

src/main/deploy/pathplanner/autos/HubCenter-Score-PushLeft.auto

@@ -62,6 +62,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubCenter",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/HubCenter-Score-PushRight.auto

@@ -62,6 +62,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubCenter",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/HubCenter-Score.auto

@@ -32,6 +32,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubCenter",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/HubCenter-Station-Score.auto

@@ -51,6 +51,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubCenter",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/HubFarLeft-Score-Push.auto

@@ -57,6 +57,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubLeft",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/HubFarRight-Score-Push.auto

@@ -57,6 +57,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubRight",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/HubLeft-Score.auto

@@ -32,6 +32,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubLeft",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/HubRight-Score.auto

@@ -32,6 +32,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubRight",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/HubRight-Station-Score-Push.auto

@@ -75,6 +75,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubRight",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/HubRight-Station-Score.auto

@@ -51,6 +51,6 @@
     }
   },
   "resetOdom": true,
-  "folder": "HubRight",
+  "folder": "Pikes Peak Autos",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/Test-LClimb.auto

@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Climber Extended"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.2
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftTower"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.2
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Climber Retracted"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Test-RClimb.auto

@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Climber Extended"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.2
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightTower"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.2
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Climber Retracted"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot-Neutral-Shoot.auto

@@ -0,0 +1,123 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LeftBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftBump-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LeftBump2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftBump2-Neutral2-Shoot2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. LeftRamp-Neutral-Shoot.auto

@@ -0,0 +1,68 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LeftBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftBump-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto

@@ -0,0 +1,55 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftTrench-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot-Neutral-Shoot.auto

@@ -0,0 +1,123 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RightBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightBump-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RightBump2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightBump2-Neutral2-Shoot2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. RightRamp-Neutral-Shoot.auto

@@ -0,0 +1,68 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RightBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Extended"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightBump-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto

@@ -0,0 +1,49 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightTrench-Neutral-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetBackward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RightRamp-Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/LeftBump-Neutral-Shoot.path

@@ -0,0 +1,128 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.968,
+        "y": 5.709
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.105960476887348,
+        "y": 5.9174871862178
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.781615384615385,
+        "y": 6.976935897435897
+      },
+      "prevControl": {
+        "x": 7.478867821173717,
+        "y": 7.355370351737978
+      },
+      "nextControl": {
+        "x": 7.9377891465039925,
+        "y": 6.78171869507514
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.781615384615385,
+        "y": 4.837346153846154
+      },
+      "prevControl": {
+        "x": 7.804249321126013,
+        "y": 5.08631945546308
+      },
+      "nextControl": {
+        "x": 7.758981448104757,
+        "y": 4.588372852229227
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 6.795888849032032,
+        "y": 5.623585450157039
+      },
+      "nextControl": {
+        "x": 6.348675253532072,
+        "y": 5.399978652407064
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 3.6429272202047827,
+        "y": 5.560811085066598
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": 112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.02657807308969639,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 113.07
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LeftBump.path

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4559230769230784,
+        "y": 5.709461538461539
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7466282051282063,
+        "y": 5.68620512820513
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.967615384615385,
+        "y": 5.709461538461539
+      },
+      "prevControl": {
+        "x": 4.746653846153847,
+        "y": 5.430384615384616
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5211640211640187,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": -91.24536426676825
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -147.0305960965379
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LeftBump2-Neutral2-Shoot2.path

@@ -0,0 +1,138 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 5.651320512820514
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.049027667109173,
+        "y": 5.401320514351452
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.7931923076864935,
+        "y": 3.453589743589744
+      },
+      "prevControl": {
+        "x": 5.724512025720087,
+        "y": 3.693970730502218
+      },
+      "nextControl": {
+        "x": 5.90029192050797,
+        "y": 3.0787410986677135
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.316487179487182,
+        "y": 4.128025641025641
+      },
+      "prevControl": {
+        "x": 7.182601673147601,
+        "y": 3.9168984964132223
+      },
+      "nextControl": {
+        "x": 7.618820512820513,
+        "y": 4.604782051282052
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.4908846153846165,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 6.716022524475761,
+        "y": 5.620469317739846
+      },
+      "nextControl": {
+        "x": 6.153666666666667,
+        "y": 5.348987179487179
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 3.6429272202047827,
+        "y": 5.560811085066598
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": 112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 6.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.02657807308969639,
+      "maxWaypointRelativePos": 0.9302325581395484,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 8.0,
+        "y": 5.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 1.2846068660022045,
+      "maxWaypointRelativePos": 1.7718715393134012,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 5.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 2.1085271317829366,
+      "maxWaypointRelativePos": 3.1893687707641063,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 113.07
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LeftBump2.path

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.2815000000000016,
+        "y": 5.511782051282052
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.735000000000001,
+        "y": 5.593179487179486
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 5.616435897435898
+      },
+      "prevControl": {
+        "x": 4.804782051282053,
+        "y": 5.616435897435898
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.45263157894736444,
+      "rotationDegrees": 112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 91.245
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": 117.25532837494306
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LeftRamp-Shoot.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.398,
+        "y": 5.512
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2417373766296596,
+        "y": 5.316853920004989
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.23498717948718,
+        "y": 5.407128205128206
+      },
+      "prevControl": {
+        "x": 3.369206052535471,
+        "y": 5.618043577061233
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.612,
+        "y": 4.021
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 133.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": 113.07
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LeftTower.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0522864367238562,
+        "y": 4.953628205128206
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.0522864367238562,
+        "y": 4.66062130060539
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0522864367238562,
+        "y": 4.662923076923078
+      },
+      "prevControl": {
+        "x": 1.0522864367238562,
+        "y": 4.912923076923078
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 4.67
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.7513725490196074,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 5.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.36705882352941094,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.1,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path

@@ -0,0 +1,152 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5373205128205143,
+        "y": 7.430435897435898
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7870116206619695,
+        "y": 7.418012103131955
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.642076923076923,
+        "y": 7.069961538461539
+      },
+      "prevControl": {
+        "x": 7.178228149431453,
+        "y": 7.5205380189223625
+      },
+      "nextControl": {
+        "x": 8.216115793872147,
+        "y": 6.51234787481454
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.781615384615385,
+        "y": 4.837346153846154
+      },
+      "prevControl": {
+        "x": 7.786585532087434,
+        "y": 5.241663699788379
+      },
+      "nextControl": {
+        "x": 7.778542446002628,
+        "y": 4.5873650404629975
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 6.957648225000392,
+        "y": 5.601152104386376
+      },
+      "nextControl": {
+        "x": 6.204516600433754,
+        "y": 5.426493771942126
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 5.511782051282052
+      },
+      "prevControl": {
+        "x": 3.7826390804558825,
+        "y": 5.585258005415613
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": 112.5
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.374307862679942,
+      "maxWaypointRelativePos": 4.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 10.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.9302325581395282,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 0.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 7.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.7353266888150656,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.5,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 101.70667085951649
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path

@@ -33,7 +33,7 @@
   "pointTowardsZones": [
     {
       "fieldPosition": {
-        "x": 4.6115541,
+        "x": 4.612,
         "y": 4.0213534
       },
       "rotationOffset": 180.0,

src/main/deploy/pathplanner/paths/RightBump-Neutral-Shoot.path

@@ -0,0 +1,128 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.968,
+        "y": 2.7089999999999996
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.900563478191006,
+        "y": 2.468267128280525
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.723474358974359,
+        "y": 0.9767820512820515
+      },
+      "prevControl": {
+        "x": 7.037410256410256,
+        "y": 1.0930641025641024
+      },
+      "nextControl": {
+        "x": 7.969958998382976,
+        "y": 0.9350049937551674
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.863012820512822,
+        "y": 3.4419615384615385
+      },
+      "prevControl": {
+        "x": 7.88564675702345,
+        "y": 3.690934840078465
+      },
+      "nextControl": {
+        "x": 7.8403788840021935,
+        "y": 3.192988236844612
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 2.5119999999999996
+      },
+      "prevControl": {
+        "x": 6.795888849032032,
+        "y": 2.623803398874987
+      },
+      "nextControl": {
+        "x": 6.348675253532072,
+        "y": 2.400196601125012
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 2.512
+      },
+      "prevControl": {
+        "x": 3.6429272202047827,
+        "y": 2.5610290337845467
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.02657807308969639,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": -113.07
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.3,
+    "rotation": 88.32335186314447
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RightBump.path

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4559230769230784,
+        "y": 2.5465897435897444
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7466282051282063,
+        "y": 2.523333333333337
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.967615384615385,
+        "y": 2.7089999999999996
+      },
+      "prevControl": {
+        "x": 4.746653846153847,
+        "y": 2.4299230769230773
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5211640211640187,
+      "rotationDegrees": 112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 91.245
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 127.62823656328939
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RightBump2-Neutral2-Shoot2.path

@@ -0,0 +1,142 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 2.5119999999999996
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.049027667109173,
+        "y": 2.2620000015309394
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 4.8140897435897445
+      },
+      "prevControl": {
+        "x": 5.8000264938666355,
+        "y": 4.791458056872672
+      },
+      "nextControl": {
+        "x": 6.297973506133364,
+        "y": 4.836721430306816
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.374628205128206,
+        "y": 4.267564102564102
+      },
+      "prevControl": {
+        "x": 7.209222795856496,
+        "y": 4.455023566405373
+      },
+      "nextControl": {
+        "x": 7.549051282051284,
+        "y": 4.069884615384615
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.7118205128205135,
+        "y": 2.779153846153846
+      },
+      "prevControl": {
+        "x": 6.936958421911658,
+        "y": 2.8878411126116403
+      },
+      "nextControl": {
+        "x": 6.374602564102564,
+        "y": 2.6163589743589735
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 2.5119999999999996
+      },
+      "prevControl": {
+        "x": 3.6429272202047827,
+        "y": 2.5610290337845445
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.18947368421052632,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 6.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.23034330011073328,
+      "maxWaypointRelativePos": 0.868217054263568,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 8.0,
+        "y": 2.5
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 1.2846068660022045,
+      "maxWaypointRelativePos": 1.7718715393134012,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 5.0,
+        "y": 0.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 2.1085271317829366,
+      "maxWaypointRelativePos": 3.1893687707641063,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": -113.07
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RightBump2.path

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5256923076923092,
+        "y": 2.6159999999999997
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8279615384615413,
+        "y": 1.9764871794871786
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.049,
+        "y": 2.6159999999999997
+      },
+      "prevControl": {
+        "x": 5.002474358974361,
+        "y": 2.488448717948718
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5211640211640187,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": -91.245
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": -108.98741511036879
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RightRamp-Shoot.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.398,
+        "y": 2.5119999999999996
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.160642585150445,
+        "y": 2.590494952805491
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.398,
+        "y": 2.5119999999999996
+      },
+      "prevControl": {
+        "x": 3.595338054344081,
+        "y": 2.3585148466212686
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.612,
+        "y": 4.021
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -129.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": -113.07
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RightTower.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0522864367238562,
+        "y": 2.4768205128205136
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.0771623664791057,
+        "y": 2.725579810373011
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0522864367238562,
+        "y": 2.8
+      },
+      "prevControl": {
+        "x": 1.0473432640619955,
+        "y": 2.5500488746894003
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 2.7
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.3999999999999999,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.1,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path

@@ -0,0 +1,165 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6070897435897447,
+        "y": 0.6046794871794885
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8833980386504074,
+        "y": 0.5273348158056345
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.595564102564104,
+        "y": 0.9884102564102579
+      },
+      "prevControl": {
+        "x": 7.151835398129718,
+        "y": 0.5429530943613806
+      },
+      "nextControl": {
+        "x": 7.93843114627953,
+        "y": 1.332612871042205
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.793243589743589,
+        "y": 3.4070769230769233
+      },
+      "prevControl": {
+        "x": 7.74538518514461,
+        "y": 3.9526634447090423
+      },
+      "nextControl": {
+        "x": 7.8344917146104915,
+        "y": 2.936847688346859
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.572282051282052,
+        "y": 2.5119999999999996
+      },
+      "prevControl": {
+        "x": 7.050706524183416,
+        "y": 2.443033938208306
+      },
+      "nextControl": {
+        "x": 6.24953920807272,
+        "y": 2.5585241728388555
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3977820512820527,
+        "y": 2.512
+      },
+      "prevControl": {
+        "x": 3.841498071206661,
+        "y": 2.6514755211571583
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 3.4736842105263164,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.2059800664451794,
+      "maxWaypointRelativePos": 4.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 10.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.771871539313429,
+      "maxWaypointRelativePos": 2.542635658914713,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 600.0,
+        "maxAngularAcceleration": 750.0,
+        "nominalVoltage": 6.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.859357696566985,
+      "maxWaypointRelativePos": 2.1705426356589252,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 7.0,
+        "y": 8.0
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 2.4983388704318887,
+      "maxWaypointRelativePos": 3.2956810631229274,
+      "name": "Point Towards Zone"
+    },
+    {
+      "fieldPosition": {
+        "x": 0.0,
+        "y": 0.6
+      },
+      "rotationOffset": 180.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 0.6910299003322272,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": true
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": -113.07
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.3,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/settings.json

@@ -4,9 +4,7 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [
-    "HubCenter",
+    "Pikes Peak Autos"
-    "HubRight",
-    "HubLeft"
   ],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,

src/main/java/frc4388/robot/RobotContainer.java

@@ -81,7 +81,7 @@ public class RobotContainer {
     //Testing of Colors
     public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
-    public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO);
+    public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO, m_robotSwerveDrive);
     public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
     
 
@@ -109,7 +109,7 @@ public class RobotContainer {
     
     
         private Command IntakeExtended = new SequentialCommandGroup(
-            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendedREMOVEME), m_robotIntake)
+            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendingRolling), m_robotIntake)
         );
     
         // private Command LidarIntake = new SequentialCommandGroup(
@@ -146,7 +146,7 @@ public class RobotContainer {
         );
     
         private Command IntakeRetracted = new SequentialCommandGroup(
-            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RetractedREMOVEME), m_robotIntake)
+            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RectractTorque), m_robotIntake)
         );
     
         private Command RobotShoot = new SequentialCommandGroup(
@@ -195,17 +195,17 @@ public class RobotContainer {
     
             NamedCommands.registerCommand("BumpOffsetForward", new InstantCommand(() -> {
                 if (TimesNegativeOne.isRed) {
-                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
+                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED_FRONT);
                 } else {
-                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
+                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE_FRONT);
                 }
             }));
 
-            NamedCommands.registerCommand("BumpOffsetReverse", new InstantCommand(() -> {
+            NamedCommands.registerCommand("BumpOffsetBackward", new InstantCommand(() -> {
-                if (!TimesNegativeOne.isRed) {
+                if (TimesNegativeOne.isRed) {
-                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
+                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED_BACK);
                 } else {
-                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
+                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE_BACK);
                 }
             }));
 
@@ -304,8 +304,8 @@ public class RobotContainer {
                     FieldPositions.HUB_POSITION,
                     ShooterConstants.AIM_LEAD_TIME.get()
                     );
-                }, m_robotSwerveDrive)
+                }, m_robotSwerveDrive))
-            );
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
 
         // D-PAD fine alignment
         new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
@@ -423,6 +423,18 @@ public class RobotContainer {
             //     m_robotIntake.setMode(IntakeMode.Idle);
             // }));
 
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
+        //     .whileTrue(
+        //         new PathPlannerAuto("Right_AutoClimb")
+        //     )
+        //     .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
+
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
+        //     .whileTrue(
+        //         new PathPlannerAuto("Left_AutoClimb")
+        //     )
+        //     .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
+
         // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
         //     .onTrue(new InstantCommand(() -> {
         //         m_robotClimber.toggleDeployed();
@@ -435,11 +447,11 @@ public class RobotContainer {
    private void configureSINGLEBindings() {
             
             //Driver controls
-            // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
-            //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
-                
             new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
-                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED)));
+                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
+                
+            // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+            //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED)));
     
             new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
                 .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
@@ -596,6 +608,7 @@ public class RobotContainer {
 
         for (String auto : autos) {
             if (auto.endsWith(".auto"))
+                if (auto.startsWith("X. "))
                     autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
             // System.out.println(auto);
         }

src/main/java/frc4388/robot/constants/FieldConstants.java

@@ -11,17 +11,28 @@ public final class FieldConstants {
     public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
     
     public static final double BUMP_OFFSET = 0.4;
-    public static final Transform2d BUMP_OFFSET_RED = new Transform2d(
+    public static final Transform2d BUMP_OFFSET_RED_FRONT = new Transform2d(
         Meters.of(BUMP_OFFSET),
         Meters.of(0),
         new Rotation2d()
     );
-    public static final Transform2d BUMP_OFFSET_BLUE = new Transform2d(
+    public static final Transform2d BUMP_OFFSET_BLUE_FRONT = new Transform2d(
         Meters.of(-BUMP_OFFSET),
         Meters.of(0),
         new Rotation2d()
     );
 
+    public static final Transform2d BUMP_OFFSET_RED_BACK = new Transform2d(
+        Meters.of(-BUMP_OFFSET),
+        Meters.of(0),
+        new Rotation2d()
+    );
+    public static final Transform2d BUMP_OFFSET_BLUE_BACK = new Transform2d(
+        Meters.of(BUMP_OFFSET),
+        Meters.of(0),
+        new Rotation2d()
+    );
+
     // Test april tag field layout
         // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
         //     Arrays.asList(new AprilTag[] {

src/main/java/frc4388/robot/subsystems/intake/Intake.java

@@ -2,7 +2,6 @@ package frc4388.robot.subsystems.intake;
 
 import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Rotations;
-import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import java.util.function.Supplier;
 
@@ -11,19 +10,23 @@ import org.littletonrobotics.junction.Logger;
 import com.ctre.phoenix6.Utils;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 public class Intake extends SubsystemBase {
     public IntakeIO io;
     IntakeStateAutoLogged state = new IntakeStateAutoLogged();
-
+    SwerveDrive m_SwerveDrive;
     Supplier<Pose2d> m_swervePoseSupplier;
 
     public Intake(
-        IntakeIO io
+        IntakeIO io,
+        SwerveDrive m_SwerveDrive
         // Supplier<Pose2d> swervePoseSupplier
     ) {
         this.io = io;
+        this.m_SwerveDrive = m_SwerveDrive;
         // this.m_swervePoseSupplier = swervePoseSupplier;
     }
 
@@ -93,8 +96,9 @@ public class Intake extends SubsystemBase {
     public void periodic() {
         // FaultReporter.register(this); // TODO Implement fault reporter
         // System.out.println(m_armLimitSwitch.get());
+        ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
 
-
+        double ChassisOverallSpeed = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond);
         Logger.processInputs("Intake", state);
         Logger.recordOutput("Intake/IntakeState", this.mode);
 
@@ -126,7 +130,7 @@ public class Intake extends SubsystemBase {
                 
             case ExtendingRolling:
                 io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
-                io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
+                io.setRollerOutput(state, IntakeConstants. ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed));
                 break;
 
             case Retracting:

src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java

@@ -6,6 +6,9 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+
 import com.revrobotics.spark.config.LimitSwitchConfig.Behavior;
 import com.revrobotics.spark.config.LimitSwitchConfig.Type;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -59,8 +62,9 @@ public class IntakeConstants {
 
     public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 1.17);
     
-    public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .20);
+    public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .50);
-    public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .20);
+    public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .40);
+    public static final ConfigurableDouble ROLLER_MULTIPLIER_CONST = new ConfigurableDouble("Roller Multiplier Constant", 0.4);
 
     // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
 
@@ -79,7 +83,9 @@ public class IntakeConstants {
     // public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
     // public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05);
     
-
+    public static double getTargetRollerSpeed(double chassisSpeed) {
+        return ROLLER_PERCENT_OUTPUT.get() * (1 + ROLLER_MULTIPLIER_CONST.get() * chassisSpeed);
+    }
 
 
     // 0 is paralell to the ground, 90 is directly up