From 4a84ec51a6e9a3e0c84d8e398022134a39aa6149 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Mon, 16 Mar 2026 22:27:15 -0400
Subject: [PATCH 001/102] get rid of all current pathplanner paths

---
 src/main/deploy/pathplanner/autos/A-CC.auto   |  56 -------
 .../deploy/pathplanner/autos/A-LC-CC.auto     |  25 ---
 src/main/deploy/pathplanner/autos/A-LSC.auto  |  19 ---
 .../deploy/pathplanner/autos/Bummmmpar.auto   |  31 ----
 .../deploy/pathplanner/autos/New Auto.auto    |  25 ---
 .../pathplanner/paths/A-Cycle-Center#.path    | 116 -------------
 .../pathplanner/paths/A-Cycle-Center.path     | 115 -------------
 .../pathplanner/paths/A-Cycle-Left#.path      | 156 -----------------
 .../pathplanner/paths/A-Cycle-Left.path       | 158 ------------------
 .../pathplanner/paths/A-Cycle-LeftSweep.path  | 158 ------------------
 .../pathplanner/paths/A-Cycle-Right#.path     | 156 -----------------
 .../pathplanner/paths/A-Cycle-Right.path      | 138 ---------------
 .../deploy/pathplanner/paths/A-backup.path    |  54 ------
 .../deploy/pathplanner/paths/Test Circle.path |  86 ----------
 .../pathplanner/paths/Test Straight.path      |  54 ------
 .../deploy/pathplanner/paths/bummmparr.path   |  99 -----------
 16 files changed, 1446 deletions(-)
 delete mode 100644 src/main/deploy/pathplanner/autos/A-CC.auto
 delete mode 100644 src/main/deploy/pathplanner/autos/A-LC-CC.auto
 delete mode 100644 src/main/deploy/pathplanner/autos/A-LSC.auto
 delete mode 100644 src/main/deploy/pathplanner/autos/Bummmmpar.auto
 delete mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto
 delete mode 100644 src/main/deploy/pathplanner/paths/A-Cycle-Center#.path
 delete mode 100644 src/main/deploy/pathplanner/paths/A-Cycle-Center.path
 delete mode 100644 src/main/deploy/pathplanner/paths/A-Cycle-Left#.path
 delete mode 100644 src/main/deploy/pathplanner/paths/A-Cycle-Left.path
 delete mode 100644 src/main/deploy/pathplanner/paths/A-Cycle-LeftSweep.path
 delete mode 100644 src/main/deploy/pathplanner/paths/A-Cycle-Right#.path
 delete mode 100644 src/main/deploy/pathplanner/paths/A-Cycle-Right.path
 delete mode 100644 src/main/deploy/pathplanner/paths/A-backup.path
 delete mode 100644 src/main/deploy/pathplanner/paths/Test Circle.path
 delete mode 100644 src/main/deploy/pathplanner/paths/Test Straight.path
 delete mode 100644 src/main/deploy/pathplanner/paths/bummmparr.path

diff --git a/src/main/deploy/pathplanner/autos/A-CC.auto b/src/main/deploy/pathplanner/autos/A-CC.auto
deleted file mode 100644
index 436acac1a..000000000
--- a/src/main/deploy/pathplanner/autos/A-CC.auto
+++ /dev/null
@@ -1,56 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "deadline",
-          "data": {
-            "commands": [
-              {
-                "type": "path",
-                "data": {
-                  "pathName": "A-Cycle-Center"
-                }
-              },
-              {
-                "type": "named",
-                "data": {
-                  "name": "startIntake"
-                }
-              }
-            ]
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "endIntake"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "driveToHubAuto"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "shootAuto"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "bringInIntake"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
diff --git a/src/main/deploy/pathplanner/autos/A-LC-CC.auto b/src/main/deploy/pathplanner/autos/A-LC-CC.auto
deleted file mode 100644
index b7fef3438..000000000
--- a/src/main/deploy/pathplanner/autos/A-LC-CC.auto
+++ /dev/null
@@ -1,25 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "A-Cycle-Left"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "A-Cycle-Right#"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/A-LSC.auto b/src/main/deploy/pathplanner/autos/A-LSC.auto
deleted file mode 100644
index 48a36d011..000000000
--- a/src/main/deploy/pathplanner/autos/A-LSC.auto
+++ /dev/null
@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "A-Cycle-LeftSweep"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Bummmmpar.auto b/src/main/deploy/pathplanner/autos/Bummmmpar.auto
deleted file mode 100644
index ccf8616c2..000000000
--- a/src/main/deploy/pathplanner/autos/Bummmmpar.auto
+++ /dev/null
@@ -1,31 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "named",
-          "data": {
-            "name": "Drive Back Left"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Drive Over Left"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Drive Back Left"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
deleted file mode 100644
index f1f316df0..000000000
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ /dev/null
@@ -1,25 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Test Circle"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Test Straight"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Center#.path b/src/main/deploy/pathplanner/paths/A-Cycle-Center#.path
deleted file mode 100644
index 60cf9b1fa..000000000
--- a/src/main/deploy/pathplanner/paths/A-Cycle-Center#.path
+++ /dev/null
@@ -1,116 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 2.493756708407871,
-        "y": 5.519624329159212
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 3.3709768232979314,
-        "y": 5.466689313590882
-      },
-      "isLocked": false,
-      "linkedName": "A-end"
-    },
-    {
-      "anchor": {
-        "x": 7.604758497316638,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 7.855269447439065,
-        "y": 5.5207065008482745
-      },
-      "nextControl": {
-        "x": 6.419272864594822,
-        "y": 5.514503199767883
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.493756708407871,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 3.0725592844223644,
-        "y": 5.518868335131507
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "A-end"
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.09090909090909055,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 1.8288770053475967,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 1.8877005347593623,
-      "rotationDegrees": 0.0
-    }
-  ],
-  "constraintZones": [
-    {
-      "name": "Intake",
-      "minWaypointRelativePos": 0.6508474576271186,
-      "maxWaypointRelativePos": 0.9627118644067798,
-      "constraints": {
-        "maxVelocity": 1.2,
-        "maxAcceleration": 3.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    }
-  ],
-  "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "startIntake",
-      "waypointRelativePos": 0.46779661016949126,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "endIntake",
-      "waypointRelativePos": 1.3966101694915225,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "shootDistance",
-      "waypointRelativePos": 2.0,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -49.531177178735966
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": -49.531177178735966
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Center.path b/src/main/deploy/pathplanner/paths/A-Cycle-Center.path
deleted file mode 100644
index 04862b0b1..000000000
--- a/src/main/deploy/pathplanner/paths/A-Cycle-Center.path
+++ /dev/null
@@ -1,115 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.6446013289036543,
-        "y": 5.519624329159212
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 4.521821443793716,
-        "y": 5.466689313590882
-      },
-      "isLocked": false,
-      "linkedName": "A-initial"
-    },
-    {
-      "anchor": {
-        "x": 7.767012522361359,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 8.017523472483786,
-        "y": 5.5207065008482745
-      },
-      "nextControl": {
-        "x": 6.581526889639543,
-        "y": 5.514503199767883
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.493756708407871,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 3.0725592844223644,
-        "y": 5.518868335131507
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "A-end"
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 1.8288770053475967,
-      "rotationDegrees": 0.0
-    }
-  ],
-  "constraintZones": [
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 0.6,
-      "maxWaypointRelativePos": 0.66,
-      "constraints": {
-        "maxVelocity": 1.35,
-        "maxAcceleration": 4.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    },
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 0.66,
-      "maxWaypointRelativePos": 1.08,
-      "constraints": {
-        "maxVelocity": 0.9,
-        "maxAcceleration": 2.5,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    },
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 1.08,
-      "maxWaypointRelativePos": 1.75,
-      "constraints": {
-        "maxVelocity": 2.5,
-        "maxAcceleration": 5.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    }
-  ],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -49.531177178735966
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Left#.path b/src/main/deploy/pathplanner/paths/A-Cycle-Left#.path
deleted file mode 100644
index 187d23772..000000000
--- a/src/main/deploy/pathplanner/paths/A-Cycle-Left#.path
+++ /dev/null
@@ -1,156 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.6446013289036543,
-        "y": 5.519624329159212
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 4.8770912764137435,
-        "y": 5.519624329159212
-      },
-      "isLocked": false,
-      "linkedName": "A-initial"
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 4.73288905179687,
-        "y": 5.51228164187815
-      },
-      "nextControl": {
-        "x": 7.961717352415029,
-        "y": 5.535849731663684
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 7.750787119856888,
-        "y": 6.704078711985688
-      },
-      "prevControl": {
-        "x": 7.767012522361359,
-        "y": 5.7143291592128795
-      },
-      "nextControl": {
-        "x": 7.735900794997783,
-        "y": 7.612144528391123
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 6.210705938954184,
-        "y": 5.533502821207796
-      },
-      "nextControl": {
-        "x": 4.635509838998211,
-        "y": 5.4871735241502675
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 2.493756708407871,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 2.243756708407871,
-        "y": 5.519624329159212
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "A-end"
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.06417112299465277,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 1.0106951871657757,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 2.213903743315511,
-      "rotationDegrees": 90.0
-    },
-    {
-      "waypointRelativePos": 2.9946524064171127,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 3.689839572192517,
-      "rotationDegrees": 0.0
-    }
-  ],
-  "constraintZones": [
-    {
-      "name": "Intake",
-      "minWaypointRelativePos": 1.1796610169491524,
-      "maxWaypointRelativePos": 2.115254237288133,
-      "constraints": {
-        "maxVelocity": 1.2,
-        "maxAcceleration": 3.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    }
-  ],
-  "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "startIntake",
-      "waypointRelativePos": 1.1254237288135593,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "endIntake",
-      "waypointRelativePos": 2.250847457627117,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "shootDistance",
-      "waypointRelativePos": 4.0,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -49.531177178735966
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Left.path b/src/main/deploy/pathplanner/paths/A-Cycle-Left.path
deleted file mode 100644
index 420c9724a..000000000
--- a/src/main/deploy/pathplanner/paths/A-Cycle-Left.path
+++ /dev/null
@@ -1,158 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.6446013289036543,
-        "y": 5.519624329159212
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 4.8770912764137435,
-        "y": 5.519624329159212
-      },
-      "isLocked": false,
-      "linkedName": "A-initial"
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 4.73288905179687,
-        "y": 5.51228164187815
-      },
-      "nextControl": {
-        "x": 7.961717352415029,
-        "y": 5.535849731663684
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 7.750787119856888,
-        "y": 6.704078711985688
-      },
-      "prevControl": {
-        "x": 7.767012522361359,
-        "y": 5.7143291592128795
-      },
-      "nextControl": {
-        "x": 7.735900794997783,
-        "y": 7.612144528391123
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 6.210705938954184,
-        "y": 5.533502821207796
-      },
-      "nextControl": {
-        "x": 4.635509838998211,
-        "y": 5.4871735241502675
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 2.493756708407871,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 2.243756708407871,
-        "y": 5.519624329159212
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "A-end"
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 1.0106951871657757,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 2.213903743315511,
-      "rotationDegrees": 90.0
-    },
-    {
-      "waypointRelativePos": 2.9946524064171127,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 3.689839572192517,
-      "rotationDegrees": 0.0
-    }
-  ],
-  "constraintZones": [
-    {
-      "name": "Drive",
-      "minWaypointRelativePos": 1.1254237288135591,
-      "maxWaypointRelativePos": 2.2644067796610168,
-      "constraints": {
-        "maxVelocity": 1.2,
-        "maxAcceleration": 3.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    }
-  ],
-  "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "startIntake",
-      "waypointRelativePos": 1.1254237288135593,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "endIntake",
-      "waypointRelativePos": 2.250847457627117,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "preloadBalls",
-      "waypointRelativePos": 2.7254237288135585,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "shootDistance",
-      "waypointRelativePos": 4.0,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -49.531177178735966
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-LeftSweep.path b/src/main/deploy/pathplanner/paths/A-Cycle-LeftSweep.path
deleted file mode 100644
index b4b732b20..000000000
--- a/src/main/deploy/pathplanner/paths/A-Cycle-LeftSweep.path
+++ /dev/null
@@ -1,158 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.6446013289036543,
-        "y": 5.519624329159212
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 4.644601328903654,
-        "y": 5.519624329159212
-      },
-      "isLocked": false,
-      "linkedName": "A-initial"
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 4.738967256770203,
-        "y": 5.503497394440951
-      },
-      "nextControl": {
-        "x": 6.744812164579606,
-        "y": 5.535849731663685
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 7.507406082289803,
-        "y": 7.093488372093023
-      },
-      "prevControl": {
-        "x": 7.00742058048248,
-        "y": 7.076822188699445
-      },
-      "nextControl": {
-        "x": 7.994168157423971,
-        "y": 7.109713774597496
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.718336314847942,
-        "y": 4.57855098389982
-      },
-      "prevControl": {
-        "x": 7.443998211091234,
-        "y": 4.570438282647584
-      },
-      "nextControl": {
-        "x": 7.9926744186046506,
-        "y": 4.586663685152057
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 5.988346608309651,
-        "y": 5.50396994296864
-      },
-      "nextControl": {
-        "x": 5.489327810295001,
-        "y": 5.535278715349785
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 2.493756708407871,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 2.244321250822382,
-        "y": 5.502832887913206
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "A-end"
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 1.0000000000000002,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 2.0454545454545476,
-      "rotationDegrees": -90.0
-    },
-    {
-      "waypointRelativePos": 4.6791443850267385,
-      "rotationDegrees": -90.0
-    }
-  ],
-  "constraintZones": [
-    {
-      "name": "intake",
-      "minWaypointRelativePos": 1.6271186440677965,
-      "maxWaypointRelativePos": 2.796610169491525,
-      "constraints": {
-        "maxVelocity": 1.5,
-        "maxAcceleration": 6.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    },
-    {
-      "name": "bump",
-      "minWaypointRelativePos": 3.796610169491523,
-      "maxWaypointRelativePos": 4.389830508474569,
-      "constraints": {
-        "maxVelocity": 2.4,
-        "maxAcceleration": 6.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    }
-  ],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -49.531177178735966
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Right#.path b/src/main/deploy/pathplanner/paths/A-Cycle-Right#.path
deleted file mode 100644
index 14c74d019..000000000
--- a/src/main/deploy/pathplanner/paths/A-Cycle-Right#.path
+++ /dev/null
@@ -1,156 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 2.493756708407871,
-        "y": 5.519624329159212
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 4.6030590339892665,
-        "y": 5.535849731663685
-      },
-      "isLocked": false,
-      "linkedName": "A-end"
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 4.820002561980206,
-        "y": 5.514850828351657
-      },
-      "nextControl": {
-        "x": 7.704960966476037,
-        "y": 5.529838673350493
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 7.783237924865832,
-        "y": 4.57855098389982
-      },
-      "prevControl": {
-        "x": 7.864364937388192,
-        "y": 4.335169946332736
-      },
-      "nextControl": {
-        "x": 7.505477705640591,
-        "y": 5.411831641575549
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 6.435774438518339,
-        "y": 5.519624329159212
-      },
-      "nextControl": {
-        "x": 3.2076744186046513,
-        "y": 5.519624329159212
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 2.493756708407871,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 4.992468694096601,
-        "y": 5.519624329159212
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "A-end"
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.042780748663101734,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 1.0026737967914436,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 1.9518716577540105,
-      "rotationDegrees": -90.0
-    },
-    {
-      "waypointRelativePos": 2.9812834224599016,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 3.9572192513369,
-      "rotationDegrees": 0.0
-    }
-  ],
-  "constraintZones": [
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 1.193220338983051,
-      "maxWaypointRelativePos": 1.8169491525423729,
-      "constraints": {
-        "maxVelocity": 1.5,
-        "maxAcceleration": 3.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    }
-  ],
-  "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "startIntake",
-      "waypointRelativePos": 1.1254237288135593,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "endIntake",
-      "waypointRelativePos": 2.250847457627117,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "shootDistance",
-      "waypointRelativePos": 3.5,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -49.531177178735966
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": -49.531177178735966
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Right.path b/src/main/deploy/pathplanner/paths/A-Cycle-Right.path
deleted file mode 100644
index 373bdb620..000000000
--- a/src/main/deploy/pathplanner/paths/A-Cycle-Right.path
+++ /dev/null
@@ -1,138 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.6446013289036543,
-        "y": 5.519624329159212
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 5.75390365448505,
-        "y": 5.535849731663685
-      },
-      "isLocked": false,
-      "linkedName": "A-initial"
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 4.820002561980206,
-        "y": 5.514850828351657
-      },
-      "nextControl": {
-        "x": 7.704960966476037,
-        "y": 5.529838673350493
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 7.783237924865832,
-        "y": 4.57855098389982
-      },
-      "prevControl": {
-        "x": 7.864364937388192,
-        "y": 4.335169946332736
-      },
-      "nextControl": {
-        "x": 7.505477705640591,
-        "y": 5.411831641575549
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.738837209302326,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 6.435774438518339,
-        "y": 5.519624329159212
-      },
-      "nextControl": {
-        "x": 3.2076744186046513,
-        "y": 5.519624329159212
-      },
-      "isLocked": false,
-      "linkedName": "overBump"
-    },
-    {
-      "anchor": {
-        "x": 2.493756708407871,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 4.992468694096601,
-        "y": 5.519624329159212
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "A-end"
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 1.0026737967914436,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 1.9518716577540105,
-      "rotationDegrees": -90.0
-    },
-    {
-      "waypointRelativePos": 2.9812834224599016,
-      "rotationDegrees": 0.0
-    },
-    {
-      "waypointRelativePos": 3.946524064171123,
-      "rotationDegrees": 0.0
-    }
-  ],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "startIntake",
-      "waypointRelativePos": 1.1254237288135593,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "endIntake",
-      "waypointRelativePos": 2.250847457627117,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "shootDistance",
-      "waypointRelativePos": 3.5,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -49.531177178735966
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/A-backup.path b/src/main/deploy/pathplanner/paths/A-backup.path
deleted file mode 100644
index 8b2593371..000000000
--- a/src/main/deploy/pathplanner/paths/A-backup.path
+++ /dev/null
@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.6446013289036543,
-        "y": 5.519624329159212
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 3.2053024802284984,
-        "y": 5.526804970948405
-      },
-      "isLocked": false,
-      "linkedName": "A-initial"
-    },
-    {
-      "anchor": {
-        "x": 3.1624750830564783,
-        "y": 5.519624329159212
-      },
-      "prevControl": {
-        "x": 4.3413732420324855,
-        "y": 5.5414336122651005
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "A-start"
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Test Circle.path b/src/main/deploy/pathplanner/paths/Test Circle.path
deleted file mode 100644
index 1d934df2b..000000000
--- a/src/main/deploy/pathplanner/paths/Test Circle.path	
+++ /dev/null
@@ -1,86 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.659667774086378,
-        "y": 5.55671096345515
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 3.072076411960133,
-        "y": 4.697923588039868
-      },
-      "isLocked": false,
-      "linkedName": "test"
-    },
-    {
-      "anchor": {
-        "x": 1.9119601328903655,
-        "y": 5.55671096345515
-      },
-      "prevControl": {
-        "x": 2.0025193701365023,
-        "y": 5.058635158601398
-      },
-      "nextControl": {
-        "x": 1.7914285714285714,
-        "y": 6.219634551495017
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.7556810631229234,
-        "y": 7.409883720930233
-      },
-      "prevControl": {
-        "x": 2.532074265372944,
-        "y": 7.2980803220552435
-      },
-      "nextControl": {
-        "x": 3.5090033222591357,
-        "y": 7.786544850498338
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.659667774086378,
-        "y": 5.55671096345515
-      },
-      "prevControl": {
-        "x": 3.4790073097994525,
-        "y": 5.38390530196331
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Test Straight.path b/src/main/deploy/pathplanner/paths/Test Straight.path
deleted file mode 100644
index 2a18876c5..000000000
--- a/src/main/deploy/pathplanner/paths/Test Straight.path	
+++ /dev/null
@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 3.659667774086378,
-        "y": 5.55671096345515
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 4.052190889030067,
-        "y": 5.547177274221312
-      },
-      "isLocked": false,
-      "linkedName": "test"
-    },
-    {
-      "anchor": {
-        "x": 1.4900996677740863,
-        "y": 5.55671096345515
-      },
-      "prevControl": {
-        "x": 2.321660995998263,
-        "y": 5.55732099793068
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/bummmparr.path b/src/main/deploy/pathplanner/paths/bummmparr.path
deleted file mode 100644
index 1fdc53902..000000000
--- a/src/main/deploy/pathplanner/paths/bummmparr.path
+++ /dev/null
@@ -1,99 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 6.714,
-        "y": 5.44
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 6.630224274935788,
-        "y": 4.48569717198202
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.770951011344179,
-        "y": 5.034189988227739
-      },
-      "prevControl": {
-        "x": 7.762339859172713,
-        "y": 3.075686618320331
-      },
-      "nextControl": {
-        "x": 7.778291751391267,
-        "y": 6.703753344392124
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.714,
-        "y": 6.520234843214897
-      },
-      "prevControl": {
-        "x": 7.686994659191226,
-        "y": 7.3938356083891374
-      },
-      "nextControl": {
-        "x": 5.7410053408087744,
-        "y": 5.646634078040657
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.384828499571918,
-        "y": 5.663552279537671
-      },
-      "prevControl": {
-        "x": 4.233927654109589,
-        "y": 5.673683714148116
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0.9965843900966185,
-      "rotationDegrees": 90.0
-    },
-    {
-      "waypointRelativePos": 1.95,
-      "rotationDegrees": -179.9724768467094
-    },
-    {
-      "waypointRelativePos": 2.35,
-      "rotationDegrees": 180.0
-    }
-  ],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -179.60897423494995
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "useDefaultConstraints": true
-}
\ No newline at end of file

From eb0a04ef9eea25041323419daddf7d2c425d0396 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Mon, 16 Mar 2026 23:02:57 -0400
Subject: [PATCH 002/102] a-cycle and b cycle added

---
 .../deploy/pathplanner/paths/A-Cycle1.path    | 151 ++++++++++++++++++
 .../deploy/pathplanner/paths/B-Cycle1.path    | 135 ++++++++++++++++
 2 files changed, 286 insertions(+)
 create mode 100644 src/main/deploy/pathplanner/paths/A-Cycle1.path
 create mode 100644 src/main/deploy/pathplanner/paths/B-Cycle1.path

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
new file mode 100644
index 000000000..6f85ae669
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -0,0 +1,151 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6619856887298745,
+        "y": 7.35309481216458
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.997782737262628,
+        "y": 7.304739989345628
+      },
+      "isLocked": false,
+      "linkedName": "A-trench"
+    },
+    {
+      "anchor": {
+        "x": 5.398103756708408,
+        "y": 7.35309481216458
+      },
+      "prevControl": {
+        "x": 4.971598409835986,
+        "y": 7.437857906116374
+      },
+      "nextControl": {
+        "x": 6.27078220569555,
+        "y": 7.179659881269494
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.767012522361359,
+        "y": 6.655402504472272
+      },
+      "prevControl": {
+        "x": 7.4615706724585324,
+        "y": 7.038425733356718
+      },
+      "nextControl": {
+        "x": 7.947058547062239,
+        "y": 6.429625291065925
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.393828264758497,
+        "y": 4.513649373881932
+      },
+      "prevControl": {
+        "x": 7.727422463620992,
+        "y": 4.598054971585738
+      },
+      "nextControl": {
+        "x": 6.7686925268014395,
+        "y": 4.355478291206644
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.690161001788908,
+        "y": 7.35309481216458
+      },
+      "prevControl": {
+        "x": 6.200086981759959,
+        "y": 6.951888946142024
+      },
+      "nextControl": {
+        "x": 5.493684362210888,
+        "y": 7.507681131417379
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3699284436493735,
+        "y": 7.223291592128801
+      },
+      "prevControl": {
+        "x": 3.9387992839233172,
+        "y": 7.44624735096346
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7620320855614973,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.9331550802139064,
+      "rotationDegrees": -106.336
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.585,
+        "y": 4.005
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 4.3559322033898304,
+      "maxWaypointRelativePos": 5.0,
+      "name": "Hub"
+    }
+  ],
+  "eventMarkers": [
+    {
+      "name": "startIntake",
+      "waypointRelativePos": 0.7423728813559323,
+      "endWaypointRelativePos": null,
+      "command": null
+    },
+    {
+      "name": "preloadBalls",
+      "waypointRelativePos": 3.779661016949158,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 6.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -68.87528085392752
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
new file mode 100644
index 000000000..01e99613b
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -0,0 +1,135 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6457602862254017,
+        "y": 0.603327370304114
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.645760286225405,
+        "y": 0.603327370304114
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.117996422182468,
+        "y": 1.154991055456171
+      },
+      "prevControl": {
+        "x": 6.5014311270125225,
+        "y": 0.5871019677996414
+      },
+      "nextControl": {
+        "x": 7.734561717352414,
+        "y": 1.7228801431127008
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.539856887298749,
+        "y": 3.637477638640429
+      },
+      "prevControl": {
+        "x": 8.075295169946333,
+        "y": 3.5563506261180677
+      },
+      "nextControl": {
+        "x": 7.004418604651164,
+        "y": 3.7186046511627904
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.112021466905188,
+        "y": 1.5281753130590334
+      },
+      "prevControl": {
+        "x": 6.092572868677908,
+        "y": 1.7810070900136763
+      },
+      "nextControl": {
+        "x": 6.225599284436494,
+        "y": 0.05166368515205688
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.3212522361359564,
+        "y": 0.7980322003577818
+      },
+      "prevControl": {
+        "x": 3.5677075285819884,
+        "y": 0.7560823633456908
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.171122994652407,
+      "rotationDegrees": 65.4874859733465
+    },
+    {
+      "waypointRelativePos": 1.9411764705882377,
+      "rotationDegrees": 115.5063543100694
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 4.615,
+        "y": 4.045
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 3.620338983050851,
+      "maxWaypointRelativePos": 4.0,
+      "name": "Hub"
+    }
+  ],
+  "eventMarkers": [
+    {
+      "name": "startIntake",
+      "waypointRelativePos": 0.5288135593220339,
+      "endWaypointRelativePos": null,
+      "command": null
+    },
+    {
+      "name": "preloadBalls",
+      "waypointRelativePos": 2.535593220338976,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 6.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 67.83365417791755
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

From f16f874a9658f7dd528bf5d6236808c63bdb3efd Mon Sep 17 00:00:00 2001
From: Anvi Jain <anvijain51@gmail.com>
Date: Fri, 20 Mar 2026 16:02:40 -0400
Subject: [PATCH 003/102] removed indexer limit switches and preloading
 functionality

---
 src/main/java/frc/robot/Orchestrator.java     | 10 -----
 src/main/java/frc/robot/RobotContainer.java   |  4 +-
 .../java/frc/robot/commands/auto/Autos.java   | 44 +++++--------------
 .../subsystems/indexer/IndexConstants.java    |  1 -
 .../frc/robot/subsystems/indexer/Indexer.java | 22 ----------
 .../robot/subsystems/indexer/IndexerIO.java   | 10 -----
 .../subsystems/indexer/IndexerIOSparkMax.java | 21 ---------
 7 files changed, 13 insertions(+), 99 deletions(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 2e5ac8764..e7702cde7 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -32,7 +32,6 @@ public class Orchestrator {
   private final MagicCarpet magicCarpet;
   private final Indexer indexer;
   private final Intake intake;
-  private final RobotState robotState = RobotState.getInstance();
   private final ShooterLeadCompensator shooterLeadCompensator;
   private final CommandXboxController driverController;
 
@@ -140,7 +139,6 @@ public Command alignToHub() {
 
   public Command feedUp() {
     return Commands.repeatingSequence(
-            preloadBalls().until(() -> RobotState.getInstance().shooterAtSpeed),
             Commands.parallel(magicCarpet.run(), indexer.run())
                 .until(() -> !RobotState.getInstance().shooterAtSpeed))
         .onlyIf(() -> shooter.getSetpoint() > 0)
@@ -175,14 +173,6 @@ public Command spinUpShooter(double velocityRPM) {
         Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM));
   }
 
-  public Command preloadBalls() {
-    return Commands.parallel(magicCarpet.run(), indexer.runPreloadSpeeds())
-        .onlyWhile(() -> !RobotState.getInstance().indexerHasFuel)
-        .onlyIf(() -> !RobotState.getInstance().indexerHasFuel)
-        .until(() -> RobotState.getInstance().indexerHasFuel)
-        .andThen(indexer.stop().withTimeout(0.01));
-  }
-
   public Command driveShootAtAngle() {
     return Commands.parallel(
         Commands.run(
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index be8ea5a51..232b772e3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -183,9 +183,7 @@ public RobotContainer() {
     Autos autos = new Autos(drive, orchestrator, intake, shooter);
     NamedCommands.registerCommand(
         "startIntake",
-        Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), orchestrator.preloadBalls())
-            .withTimeout(10.0));
+        Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)).withTimeout(10.0));
     NamedCommands.registerCommand(
         "endIntake",
         Commands.parallel(
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 0ff07b388..9b742203b 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -102,9 +102,7 @@ public Command ACCManuelAuto() {
     return Commands.sequence(
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())),
+            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseA.get())).withTimeout(5),
         intake.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
@@ -121,9 +119,7 @@ public Command ADepot() {
             .withTimeout(2),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPose.get())).withTimeout(3.5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())),
+            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         intake.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get()))
@@ -139,18 +135,14 @@ public Command ADepot() {
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())).withTimeout(2),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(3.5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())));
+            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))));
   }
 
   public Command ACCManuelAutoAlt() {
     return Commands.sequence(
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())),
+            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get()))
             .withTimeout(3.5),
         intake.stopIntakeCommand().withTimeout(0.05),
@@ -168,9 +160,7 @@ public Command ACCManuelAutoOverBump() {
     return Commands.sequence(
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())),
+            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get()))
             .withTimeout(3.5),
         intake.stopIntakeCommand().withTimeout(0.05),
@@ -186,20 +176,16 @@ public Command ACCManuelAutoOverBump() {
         intake.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())).withTimeout(2),
-        Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(3.5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())));
+        Commands.deadline(new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())))
+            .withTimeout(3.5),
+        Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 
   public Command BCCManuelAuto() {
     return Commands.sequence(
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())),
+            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseB.get())).withTimeout(5),
         intake.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
@@ -214,9 +200,7 @@ public Command BCCManuelAutoAlt() {
     return Commands.sequence(
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())),
+            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get()))
             .withTimeout(3.5),
         intake.stopIntakeCommand().withTimeout(0.05),
@@ -234,9 +218,7 @@ public Command BCCManuelAutoOverBump() {
     return Commands.sequence(
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())),
+            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get()))
             .withTimeout(3.5),
         intake.stopIntakeCommand().withTimeout(0.05),
@@ -254,8 +236,6 @@ public Command BCCManuelAutoOverBump() {
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get())).withTimeout(2),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(3.5),
-            Commands.parallel(
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS),
-                orchestrator.preloadBalls())));
+            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexConstants.java
index b547905d1..63c232a0a 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexConstants.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexConstants.java
@@ -3,6 +3,5 @@
 public class IndexConstants {
   public static final double ENCODER_FEEDUP_POSITION_CONVERSION = 1;
   public static final double ENCODER_FEEDUP_VELOCITY_CONVERSION = 1;
-  public static final double PRELOAD_VOLT = 3.0;
   public static final double FEEDUP_VOLT = 12;
 }
diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
index 2d91cb276..215b006df 100644
--- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -1,12 +1,10 @@
 package frc.robot.subsystems.indexer;
 
 import static frc.robot.subsystems.indexer.IndexConstants.FEEDUP_VOLT;
-import static frc.robot.subsystems.indexer.IndexConstants.PRELOAD_VOLT;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.RobotState;
 import org.littletonrobotics.junction.Logger;
 
 public class Indexer extends SubsystemBase {
@@ -20,8 +18,6 @@ public Indexer(IndexerIO io) {
   @Override
   public void periodic() {
     io.updateInputs(inputs);
-    RobotState.getInstance().indexerHasFuel = inputs.ballAtLeftSwitch || inputs.ballAtRightSwitch;
-    RobotState.getInstance().indexerRunning = inputs.feedUpVolts > 0;
     Logger.processInputs("Index", inputs);
   }
 
@@ -37,14 +33,6 @@ public double getVoltage() {
     return inputs.feedUpVolts;
   }
 
-  public boolean isLeftSwitchPressed() {
-    return io.isLeftSwitchPressed();
-  }
-
-  public boolean isRightSwitchPressed() {
-    return io.isRightSwitchPressed();
-  }
-
   public Command run() {
     return Commands.run(
             () -> {
@@ -55,16 +43,6 @@ public Command run() {
         .withName("run");
   }
 
-  public Command runPreloadSpeeds() {
-    return Commands.run(
-            () -> {
-              setVoltage(PRELOAD_VOLT);
-            },
-            this)
-        .finallyDo(this::stop)
-        .withName("runPeriodic");
-  }
-
   public Command reverse() {
     return Commands.run(
             () -> {
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
index f03f1af2b..9dad526d8 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
@@ -10,8 +10,6 @@ class IndexerIOInputs {
     public double feedUpPercentOutput = 0.0;
     public double feedUpVolts = 0.0;
     public double feedUpAmps = 0.0;
-    public boolean ballAtLeftSwitch = false;
-    public boolean ballAtRightSwitch = false;
   }
 
   default void updateInputs(IndexerIOInputs inputs) {}
@@ -21,12 +19,4 @@ default void setPercent(double indexPercent, double feedUpPercent) {}
   default void setVoltage(double feedUpVolts) {}
 
   default void stop() {}
-
-  default boolean isLeftSwitchPressed() {
-    return false;
-  }
-
-  default boolean isRightSwitchPressed() {
-    return false;
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
index a532d67d1..090e6cd5d 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
@@ -1,8 +1,6 @@
 package frc.robot.subsystems.indexer;
 
 import static frc.robot.Schematic.indexerFeedupCanId;
-import static frc.robot.Schematic.indexerLeftLimitSwitchDIO;
-import static frc.robot.Schematic.indexerRightLimitSwitchDIO;
 import static frc.robot.subsystems.indexer.IndexConstants.ENCODER_FEEDUP_POSITION_CONVERSION;
 import static frc.robot.subsystems.indexer.IndexConstants.ENCODER_FEEDUP_VELOCITY_CONVERSION;
 
@@ -13,15 +11,11 @@
 import com.revrobotics.spark.config.EncoderConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.wpilibj.DigitalInput;
 
 public class IndexerIOSparkMax implements IndexerIO {
 
   private final SparkMax feedUpMotor;
 
-  private final DigitalInput leftLimitSwitch;
-  private final DigitalInput rightLimitSwitch;
-
   public IndexerIOSparkMax() {
     feedUpMotor = new SparkMax(indexerFeedupCanId, MotorType.kBrushless);
 
@@ -42,9 +36,6 @@ public IndexerIOSparkMax() {
 
     feedUpMotor.configure(
         feedUpConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-    leftLimitSwitch = new DigitalInput(indexerLeftLimitSwitchDIO);
-    rightLimitSwitch = new DigitalInput(indexerRightLimitSwitchDIO);
   }
 
   @Override
@@ -52,8 +43,6 @@ public void updateInputs(IndexerIOInputs inputs) {
     inputs.feedUpPercentOutput = feedUpMotor.get();
     inputs.feedUpVolts = feedUpMotor.getBusVoltage() * feedUpMotor.getAppliedOutput();
     inputs.feedUpAmps = feedUpMotor.getOutputCurrent();
-    inputs.ballAtLeftSwitch = !leftLimitSwitch.get();
-    inputs.ballAtRightSwitch = !rightLimitSwitch.get();
   }
 
   @Override
@@ -70,14 +59,4 @@ public void setVoltage(double feedUpVolts) {
   public void stop() {
     feedUpMotor.set(0);
   }
-
-  @Override
-  public boolean isLeftSwitchPressed() {
-    return !leftLimitSwitch.get();
-  }
-
-  @Override
-  public boolean isRightSwitchPressed() {
-    return rightLimitSwitch.get();
-  }
 }

From b661ed98a42c8c63717376c1fb97952002029a10 Mon Sep 17 00:00:00 2001
From: Anvi Jain <anvijain51@gmail.com>
Date: Fri, 20 Mar 2026 16:51:22 -0400
Subject: [PATCH 004/102] fixed changes

---
 src/main/java/frc/robot/RobotContainer.java      |  2 +-
 src/main/java/frc/robot/Schematic.java           | 14 ++++----------
 src/main/java/frc/robot/commands/auto/Autos.java |  2 +-
 3 files changed, 6 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 232b772e3..e8be5b1c4 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -183,7 +183,7 @@ public RobotContainer() {
     Autos autos = new Autos(drive, orchestrator, intake, shooter);
     NamedCommands.registerCommand(
         "startIntake",
-        Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)).withTimeout(10.0));
+        intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(10.0));
     NamedCommands.registerCommand(
         "endIntake",
         Commands.parallel(
diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java
index 9f4449875..e18de55d0 100644
--- a/src/main/java/frc/robot/Schematic.java
+++ b/src/main/java/frc/robot/Schematic.java
@@ -28,8 +28,7 @@ public class Schematic {
 
   //  indexer subsystem
   public static final int indexerFeedupCanId;
-  public static final int indexerLeftLimitSwitchDIO;
-  public static final int indexerRightLimitSwitchDIO;
+
 
   // intake subsystem
   public static final int intakeMotorCanId;
@@ -71,8 +70,7 @@ public class Schematic {
 
         // Indexer (CAN IDs and DIOs)
         indexerFeedupCanId = 0;
-        indexerLeftLimitSwitchDIO = 0;
-        indexerRightLimitSwitchDIO = 0;
+
 
         // Intake (CAN IDs)
         intakeMotorCanId = 0;
@@ -112,8 +110,7 @@ public class Schematic {
 
         // Indexer (CAN Ids)
         indexerFeedupCanId = 9;
-        indexerLeftLimitSwitchDIO = 0;
-        indexerRightLimitSwitchDIO = 2;
+
 
         // Intake (CAN IDs)
         intakeMotorCanId = 23;
@@ -152,8 +149,7 @@ public class Schematic {
 
         // Indexer (CAN IDs and DIOs)
         indexerFeedupCanId = 0;
-        indexerLeftLimitSwitchDIO = 0;
-        indexerRightLimitSwitchDIO = 2;
+
 
         // Intake (CAN IDs)
         intakeMotorCanId = 0;
@@ -192,8 +188,6 @@ public class Schematic {
 
         // Indexer (CAN IDs and DIOs)
         indexerFeedupCanId = 0;
-        indexerLeftLimitSwitchDIO = 0;
-        indexerRightLimitSwitchDIO = 0;
 
         // Intake (CAN IDs)
         intakeMotorCanId = 0;
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 9b742203b..5e8a1925b 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -135,7 +135,7 @@ public Command ADepot() {
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())).withTimeout(2),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(3.5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))));
+            (intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))));
   }
 
   public Command ACCManuelAutoAlt() {

From dcabdd589f8febc25791dbb741d7e187b4dde65c Mon Sep 17 00:00:00 2001
From: Anvi Jain <anvijain51@gmail.com>
Date: Fri, 20 Mar 2026 16:52:47 -0400
Subject: [PATCH 005/102] fixed changes

---
 src/main/java/frc/robot/RobotContainer.java | 3 +--
 src/main/java/frc/robot/Schematic.java      | 4 ----
 2 files changed, 1 insertion(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e8be5b1c4..aadf1b4e3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -182,8 +182,7 @@ public RobotContainer() {
     orchestrator = new Orchestrator(drive, magicCarpet, shooter, indexer, intake, driverController);
     Autos autos = new Autos(drive, orchestrator, intake, shooter);
     NamedCommands.registerCommand(
-        "startIntake",
-        intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(10.0));
+        "startIntake", intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(10.0));
     NamedCommands.registerCommand(
         "endIntake",
         Commands.parallel(
diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java
index e18de55d0..6d09a11d8 100644
--- a/src/main/java/frc/robot/Schematic.java
+++ b/src/main/java/frc/robot/Schematic.java
@@ -29,7 +29,6 @@ public class Schematic {
   //  indexer subsystem
   public static final int indexerFeedupCanId;
 
-
   // intake subsystem
   public static final int intakeMotorCanId;
   public static final int intakeExtendCanId;
@@ -71,7 +70,6 @@ public class Schematic {
         // Indexer (CAN IDs and DIOs)
         indexerFeedupCanId = 0;
 
-
         // Intake (CAN IDs)
         intakeMotorCanId = 0;
         intakeExtendCanId = 0;
@@ -111,7 +109,6 @@ public class Schematic {
         // Indexer (CAN Ids)
         indexerFeedupCanId = 9;
 
-
         // Intake (CAN IDs)
         intakeMotorCanId = 23;
         intakeExtendCanId = 14;
@@ -150,7 +147,6 @@ public class Schematic {
         // Indexer (CAN IDs and DIOs)
         indexerFeedupCanId = 0;
 
-
         // Intake (CAN IDs)
         intakeMotorCanId = 0;
         intakeExtendCanId = 0;

From 3bd78149b6e460371028f88477dbd6b10a2aac17 Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Wed, 25 Mar 2026 19:13:16 -0400
Subject: [PATCH 006/102] Update schematic

---
 src/main/java/frc/robot/Constants.java      |  2 +-
 src/main/java/frc/robot/RobotContainer.java |  3 +-
 src/main/java/frc/robot/Schematic.java      | 36 ++++++++-------------
 3 files changed, 17 insertions(+), 24 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 922aca595..58fb36e35 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index be8ea5a51..fdde9e04f 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -379,7 +379,8 @@ private void configureButtonBindings() {
         .toggleOnTrue(orchestrator.spinUpShooterHub());
     operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
     operatorController.y().whileTrue(indexer.reverse());
-    operatorController.x().whileTrue(intake.outtake());
+//    operatorController.x().whileTrue(intake.outtake());
+
 
     operatorController
         .leftTrigger()
diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java
index 9f4449875..289e16eda 100644
--- a/src/main/java/frc/robot/Schematic.java
+++ b/src/main/java/frc/robot/Schematic.java
@@ -28,8 +28,6 @@ public class Schematic {
 
   //  indexer subsystem
   public static final int indexerFeedupCanId;
-  public static final int indexerLeftLimitSwitchDIO;
-  public static final int indexerRightLimitSwitchDIO;
 
   // intake subsystem
   public static final int intakeMotorCanId;
@@ -71,8 +69,6 @@ public class Schematic {
 
         // Indexer (CAN IDs and DIOs)
         indexerFeedupCanId = 0;
-        indexerLeftLimitSwitchDIO = 0;
-        indexerRightLimitSwitchDIO = 0;
 
         // Intake (CAN IDs)
         intakeMotorCanId = 0;
@@ -87,20 +83,20 @@ public class Schematic {
         pigeonCanId = 17;
 
         // Drive (CAN Ids)
-        frontLeftDriveCanId = 3;
-        frontLeftTurnCanId = 4;
-        frontRightDriveCanId = 1;
-        frontRightTurnCanId = 2;
-
-        backRightDriveCanId = 7;
-        backRightTurnCanId = 8;
-        backLeftDriveCanId = 5;
-        backLeftTurnCanId = 6;
-
-        frontLeftAbsoluteEncoderCanId = 19;
-        frontRightAbsoluteEncoderCanId = 18;
-        backRightAbsoluteEncoderCanId = 20;
-        backLeftAbsoluteEncoderCanId = 21;
+        frontLeftDriveCanId = 1;
+        frontLeftTurnCanId = 2;
+        frontRightDriveCanId = 3;
+        frontRightTurnCanId = 4;
+
+        backRightDriveCanId = 5;
+        backRightTurnCanId = 6;
+        backLeftDriveCanId = 7;
+        backLeftTurnCanId = 8;
+
+        frontLeftAbsoluteEncoderCanId = 18;
+        frontRightAbsoluteEncoderCanId = 19;
+        backRightAbsoluteEncoderCanId = 21;
+        backLeftAbsoluteEncoderCanId = 20;
 
         // Shooter (CAN Ids)
         shooterTopMotorCanId = 11;
@@ -112,8 +108,6 @@ public class Schematic {
 
         // Indexer (CAN Ids)
         indexerFeedupCanId = 9;
-        indexerLeftLimitSwitchDIO = 0;
-        indexerRightLimitSwitchDIO = 2;
 
         // Intake (CAN IDs)
         intakeMotorCanId = 23;
@@ -152,8 +146,6 @@ public class Schematic {
 
         // Indexer (CAN IDs and DIOs)
         indexerFeedupCanId = 0;
-        indexerLeftLimitSwitchDIO = 0;
-        indexerRightLimitSwitchDIO = 2;
 
         // Intake (CAN IDs)
         intakeMotorCanId = 0;

From 029ba86ada5a9317660244b68e015966954f55ee Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Wed, 25 Mar 2026 19:18:00 -0400
Subject: [PATCH 007/102] Format

---
 src/main/java/frc/robot/RobotContainer.java | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1b1452e4e..a42f0bbc1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -376,8 +376,7 @@ private void configureButtonBindings() {
         .toggleOnTrue(orchestrator.spinUpShooterHub());
     operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
     operatorController.y().whileTrue(indexer.reverse());
-//    operatorController.x().whileTrue(intake.outtake());
-
+    //    operatorController.x().whileTrue(intake.outtake());
 
     operatorController
         .leftTrigger()

From f719f4b6a06ed74bf39265ad316e6203d18fc965 Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Wed, 25 Mar 2026 19:59:39 -0400
Subject: [PATCH 008/102] Made autos go closer to the middle line

---
 src/main/deploy/pathplanner/navgrid.json      | 1576 +----------------
 .../deploy/pathplanner/paths/A-Cycle1.path    |   48 +-
 .../deploy/pathplanner/paths/B-Cycle1.path    |   26 +-
 3 files changed, 38 insertions(+), 1612 deletions(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 660ca5241..ac5f52168 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1,1575 +1 @@
-{
-  "field_size": {
-    "x": 16.54,
-    "y": 8.07
-  },
-  "nodeSizeMeters": 0.3,
-  "grid": [
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ]
-  ]
-}
+{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index 6f85ae669..047a32cfd 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.997782737262628,
-        "y": 7.304739989345628
+        "x": 5.023996536488359,
+        "y": 7.372382174256761
       },
       "isLocked": false,
       "linkedName": "A-trench"
@@ -20,44 +20,44 @@
         "y": 7.35309481216458
       },
       "prevControl": {
-        "x": 4.971598409835986,
-        "y": 7.437857906116374
+        "x": 4.853359758746704,
+        "y": 7.4333678569314285
       },
       "nextControl": {
-        "x": 6.27078220569555,
-        "y": 7.179659881269494
+        "x": 6.13088641115691,
+        "y": 7.24511253967509
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.767012522361359,
-        "y": 6.655402504472272
+        "x": 7.963113468715122,
+        "y": 6.645209129630746
       },
       "prevControl": {
-        "x": 7.4615706724585324,
-        "y": 7.038425733356718
+        "x": 7.701961706280031,
+        "y": 6.985217185332927
       },
       "nextControl": {
-        "x": 7.947058547062239,
-        "y": 6.429625291065925
+        "x": 8.191055623727381,
+        "y": 6.348438515426881
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.393828264758497,
-        "y": 4.513649373881932
+        "x": 7.7068785174481285,
+        "y": 4.5489918830178
       },
       "prevControl": {
-        "x": 7.727422463620992,
-        "y": 4.598054971585738
+        "x": 8.128187503978547,
+        "y": 4.672968428198327
       },
       "nextControl": {
-        "x": 6.7686925268014395,
-        "y": 4.355478291206644
+        "x": 7.089374736141768,
+        "y": 4.367282050353818
       },
       "isLocked": false,
       "linkedName": null
@@ -68,12 +68,12 @@
         "y": 7.35309481216458
       },
       "prevControl": {
-        "x": 6.200086981759959,
-        "y": 6.951888946142024
+        "x": 6.265694051261446,
+        "y": 6.928023886217352
       },
       "nextControl": {
-        "x": 5.493684362210888,
-        "y": 7.507681131417379
+        "x": 5.489063021944757,
+        "y": 7.5016195660024545
       },
       "isLocked": false,
       "linkedName": null
@@ -84,8 +84,8 @@
         "y": 7.223291592128801
       },
       "prevControl": {
-        "x": 3.9387992839233172,
-        "y": 7.44624735096346
+        "x": 3.929443246590092,
+        "y": 7.438867658239059
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index 01e99613b..d187d214b 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -16,32 +16,32 @@
     },
     {
       "anchor": {
-        "x": 7.117996422182468,
-        "y": 1.154991055456171
+        "x": 7.343222070669198,
+        "y": 1.2331899073216823
       },
       "prevControl": {
-        "x": 6.5014311270125225,
-        "y": 0.5871019677996414
+        "x": 6.726656775499253,
+        "y": 0.6653008196651526
       },
       "nextControl": {
-        "x": 7.734561717352414,
-        "y": 1.7228801431127008
+        "x": 7.9597873658391425,
+        "y": 1.801078994978212
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.539856887298749,
-        "y": 3.637477638640429
+        "x": 7.70378850168996,
+        "y": 3.5794281843111113
       },
       "prevControl": {
-        "x": 8.075295169946333,
-        "y": 3.5563506261180677
+        "x": 8.239226784337545,
+        "y": 3.49830117178875
       },
       "nextControl": {
-        "x": 7.004418604651164,
-        "y": 3.7186046511627904
+        "x": 7.168350219042376,
+        "y": 3.6605551968334726
       },
       "isLocked": false,
       "linkedName": null
@@ -79,7 +79,7 @@
   "rotationTargets": [
     {
       "waypointRelativePos": 1.171122994652407,
-      "rotationDegrees": 65.4874859733465
+      "rotationDegrees": 86.315500893566
     },
     {
       "waypointRelativePos": 1.9411764705882377,

From abe3081181d69fe300219a5e9392a1c0220d94a6 Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Wed, 25 Mar 2026 20:03:27 -0400
Subject: [PATCH 009/102] Formatting

---
 src/main/deploy/pathplanner/navgrid.json | 1576 +++++++++++++++++++++-
 1 file changed, 1575 insertions(+), 1 deletion(-)

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index ac5f52168..660ca5241 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1,1575 @@
-{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{
+  "field_size": {
+    "x": 16.54,
+    "y": 8.07
+  },
+  "nodeSizeMeters": 0.3,
+  "grid": [
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ]
+  ]
+}

From 1bced51049bf1f806ffec4eade492e58a92eb03d Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Fri, 3 Apr 2026 21:35:03 -0400
Subject: [PATCH 010/102] Fix CANcoder matching.

---
 src/main/java/frc/robot/Schematic.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java
index 4a40c201d..38d064911 100644
--- a/src/main/java/frc/robot/Schematic.java
+++ b/src/main/java/frc/robot/Schematic.java
@@ -95,8 +95,8 @@ public class Schematic {
 
         frontLeftAbsoluteEncoderCanId = 18;
         frontRightAbsoluteEncoderCanId = 19;
-        backRightAbsoluteEncoderCanId = 21;
-        backLeftAbsoluteEncoderCanId = 20;
+        backRightAbsoluteEncoderCanId = 20;
+        backLeftAbsoluteEncoderCanId = 21;
 
         // Shooter (CAN Ids)
         shooterTopMotorCanId = 11;

From 9f1d70b11bd065e1a045d7f73917c9bb2a402fee Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Fri, 3 Apr 2026 22:38:34 -0400
Subject: [PATCH 011/102] Change to a regular robot instead of simbot.

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 58fb36e35..922aca595 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";

From 9bdb3874d86c0e2948961a2bb99115f59471a2d1 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Fri, 3 Apr 2026 20:34:08 -0400
Subject: [PATCH 012/102] drivetrian

---
 .../java/frc/robot/subsystems/drive/DriveConstants.java   | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index 82f8d896e..fe564acb2 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -15,8 +15,8 @@
 public class DriveConstants {
   public static final double maxSpeedMetersPerSec = 11.8;
   public static final double odometryFrequency = 100.0; // Hz
-  public static final double trackWidth = 0.273 * 2;
-  public static final double wheelBase = 0.488;
+  public static final double trackWidth =0.5969;
+  public static final double wheelBase =0.5271;
   public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0);
   public static final Translation2d[] moduleTranslations =
       new Translation2d[] {
@@ -59,9 +59,9 @@ public class DriveConstants {
   // Drive motor configuration
   public static final SwerveModuleConstants.ClosedLoopOutputType driveClosedLoopOutput =
       ClosedLoopOutputType.Voltage;
-  public static final int driveMotorCurrentLimit = 50;
+  public static final int driveMotorCurrentLimit = 54;
   public static final double wheelRadiusMeters = Units.inchesToMeters(1.905);
-  public static final double driveMotorReduction = 8.16;
+  public static final double driveMotorReduction = 7.03;
   public static final DCMotor driveGearbox = DCMotor.getKrakenX60Foc(1);
 
   // Drive encoder configuration

From 6d3cae23fecc09f1ecaa00f2d8a40922c32df4a9 Mon Sep 17 00:00:00 2001
From: EQMoore <eqmoor26@students.shrewsbury.k12.ma.us>
Date: Wed, 8 Apr 2026 18:52:00 -0500
Subject: [PATCH 013/102] updated shooter io for four motors

---
 src/main/java/frc/robot/Schematic.java        |  35 +++--
 .../robot/subsystems/shooter/ShooterIO.java   |  22 +--
 .../subsystems/shooter/ShooterIOSparkMax.java | 142 +++++++++++-------
 3 files changed, 117 insertions(+), 82 deletions(-)

diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java
index 38d064911..1065428f2 100644
--- a/src/main/java/frc/robot/Schematic.java
+++ b/src/main/java/frc/robot/Schematic.java
@@ -20,9 +20,10 @@ public class Schematic {
   public static final int backRightAbsoluteEncoderCanId;
 
   // shooter subsystem
-  public static final int shooterBottomMotorCanId;
-  public static final int shooterTopMotorCanId;
-  public static final int shooterMiddleMotorCanId;
+  public static final int shooterTopLeftMotorCanId;
+  public static final int shooterTopRightMotorCanId;
+  public static final int shooterBottomLeftMotorCanId;
+  public static final int shooterBottomRightMotorCanId;
   // magic carpet sub-system
   public static final int magicCarpetCanId;
 
@@ -60,9 +61,10 @@ public class Schematic {
         backLeftAbsoluteEncoderCanId = 20;
 
         // Shooter (CAN Ids)
-        shooterBottomMotorCanId = 0;
-        shooterTopMotorCanId = 0;
-        shooterMiddleMotorCanId = 0;
+        shooterTopLeftMotorCanId = 0;
+        shooterTopRightMotorCanId = 0;
+        shooterBottomLeftMotorCanId = 0;
+        shooterBottomRightMotorCanId = 0;
 
         // Magic Carpet (CAN Ids)
         magicCarpetCanId = 0;
@@ -99,9 +101,10 @@ public class Schematic {
         backLeftAbsoluteEncoderCanId = 21;
 
         // Shooter (CAN Ids)
-        shooterTopMotorCanId = 11;
-        shooterMiddleMotorCanId = 12;
-        shooterBottomMotorCanId = 13;
+        shooterTopLeftMotorCanId = 11;
+        shooterTopRightMotorCanId = 12;
+        shooterBottomLeftMotorCanId = 13;
+        shooterBottomRightMotorCanId = 14;
 
         // Hopper (CAN Ids)
         magicCarpetCanId = 15;
@@ -137,9 +140,10 @@ public class Schematic {
         backRightAbsoluteEncoderCanId = 0;
 
         // Shooter (CAN Ids)
-        shooterBottomMotorCanId = 0;
-        shooterTopMotorCanId = 0;
-        shooterMiddleMotorCanId = 0;
+        shooterTopLeftMotorCanId = 0;
+        shooterTopRightMotorCanId = 0;
+        shooterBottomLeftMotorCanId = 0;
+        shooterBottomRightMotorCanId = 0;
 
         // Magic Carpet (CAN Ids)
         magicCarpetCanId = 0;
@@ -175,9 +179,10 @@ public class Schematic {
         backRightAbsoluteEncoderCanId = 0;
 
         // Shooter (CAN Ids)
-        shooterBottomMotorCanId = 0;
-        shooterTopMotorCanId = 0;
-        shooterMiddleMotorCanId = 0;
+        shooterTopLeftMotorCanId = 0;
+        shooterTopRightMotorCanId = 0;
+        shooterBottomLeftMotorCanId = 0;
+        shooterBottomRightMotorCanId = 0;
 
         // Magic Carpet (CAN Ids)
         magicCarpetCanId = 0;
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
index 03f011667..72a5219a9 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
@@ -6,17 +6,21 @@ public interface ShooterIO {
 
   @AutoLog
   class ShooterIOInputs {
-    public double middleMotorVelocityRadPerSec;
-    public double middleMotorAppliedVolts;
-    public double middleMotorCurrentAmps;
+    public double topLeftMotorVelocityRadPerSec;
+    public double topLeftMotorAppliedVolts;
+    public double topLeftMotorCurrentAmps;
 
-    public double bottomMotorVelocityRadPerSec;
-    public double bottomMotorAppliedVolts;
-    public double bottomMotorCurrentAmps;
+    public double topRightMotorVelocityRadPerSec;
+    public double topRightMotorAppliedVolts;
+    public double topRightMotorCurrentAmps;
 
-    public double topMotorVelocityRadPerSec;
-    public double topMotorAppliedVolts;
-    public double topMotorCurrentAmps;
+    public double bottomLeftMotorVelocityRadPerSec;
+    public double bottomLeftMotorAppliedVolts;
+    public double bottomLeftMotorCurrentAmps;
+
+    public double bottomRightMotorVelocityRadPerSec;
+    public double bottomRightMotorAppliedVolts;
+    public double bottomRightMotorCurrentAmps;
 
     public boolean atSetpoint = false;
     public double setpointRPM = 0;
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
index bb97cfc34..2b562a2b0 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
@@ -1,8 +1,9 @@
 package frc.robot.subsystems.shooter;
 
-import static frc.robot.Schematic.shooterBottomMotorCanId;
-import static frc.robot.Schematic.shooterMiddleMotorCanId;
-import static frc.robot.Schematic.shooterTopMotorCanId;
+import static frc.robot.Schematic.shooterBottomLeftMotorCanId;
+import static frc.robot.Schematic.shooterBottomRightMotorCanId;
+import static frc.robot.Schematic.shooterTopLeftMotorCanId;
+import static frc.robot.Schematic.shooterTopRightMotorCanId;
 import static frc.robot.subsystems.shooter.ShooterConstants.*;
 
 import com.revrobotics.RelativeEncoder;
@@ -15,94 +16,119 @@
 
 public class ShooterIOSparkMax implements ShooterIO {
 
-  private final SparkMax middleMotor;
-  private final SparkMax bottomMotor;
-  private final SparkMax topMotor;
-  private final RelativeEncoder middleMotorEncoder;
-  private final RelativeEncoder bottomMotorEncoder;
-  private final RelativeEncoder topMotorEncoder;
+  private final SparkMax topLeftMotor;
+  private final SparkMax topRightMotor;
+  private final SparkMax bottomLeftMotor;
+  private final SparkMax bottomRightMotor;
+  private final RelativeEncoder topLeftMotorEncoder;
+  private final RelativeEncoder topRightMotorEncoder;
+  private final RelativeEncoder bottomLeftMotorEncoder;
+  private final RelativeEncoder bottomRightMotorEncoder;
 
   public ShooterIOSparkMax() {
-    middleMotor = new SparkMax(shooterMiddleMotorCanId, MotorType.kBrushless);
-    bottomMotor = new SparkMax(shooterBottomMotorCanId, MotorType.kBrushless);
-    topMotor = new SparkMax(shooterTopMotorCanId, MotorType.kBrushless);
+    topLeftMotor = new SparkMax(shooterTopLeftMotorCanId, MotorType.kBrushless);
+    topRightMotor = new SparkMax(shooterTopRightMotorCanId, MotorType.kBrushless);
+    bottomLeftMotor = new SparkMax(shooterBottomLeftMotorCanId, MotorType.kBrushless);
+    bottomRightMotor = new SparkMax(shooterBottomRightMotorCanId, MotorType.kBrushless);
 
-    var middleMotorConfig = new SparkMaxConfig();
-    middleMotorConfig
+    EncoderConfig enc = new EncoderConfig();
+    enc.positionConversionFactor(ENCODER_POSITION_CONVERSION);
+    enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION);
+
+    var bottomLeftMotorConfig = new SparkMaxConfig();
+    bottomLeftMotorConfig
         .inverted(false)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
+    bottomLeftMotorConfig.apply(enc);
 
-    var bottomMotorConfig = new SparkMaxConfig();
-    bottomMotorConfig
-        .inverted(true)
+    var topLeftMotorConfig = new SparkMaxConfig();
+    topLeftMotorConfig
+        .inverted(false)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
+    topLeftMotorConfig.follow(bottomLeftMotor.getDeviceId(), false);
+    topLeftMotorConfig.apply(enc);
 
-    var topMotorConfig = new SparkMaxConfig();
-    topMotorConfig
-        .inverted(false)
+    var bottomRightMotorConfig = new SparkMaxConfig();
+    bottomRightMotorConfig
+        .inverted(true)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
+    bottomRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
+    bottomRightMotorConfig.apply(enc);
 
-    topMotorConfig.follow(bottomMotor.getDeviceId(), false);
-    middleMotorConfig.follow(bottomMotor.getDeviceId(), true);
-
-    EncoderConfig enc = new EncoderConfig();
-    enc.positionConversionFactor(ENCODER_POSITION_CONVERSION);
-    enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION);
-    middleMotorConfig.apply(enc);
-    bottomMotorConfig.apply(enc);
-    topMotorConfig.apply(enc);
-
-    middleMotor.configure(
-        middleMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-    bottomMotor.configure(
-        bottomMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-    topMotor.configure(
-        topMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-    middleMotorEncoder = middleMotor.getEncoder();
-    bottomMotorEncoder = bottomMotor.getEncoder();
-    topMotorEncoder = topMotor.getEncoder();
+    var topRightMotorConfig = new SparkMaxConfig();
+    topRightMotorConfig
+        .inverted(true)
+        .idleMode(IDLE_MODE)
+        .voltageCompensation(VOLTAGE_COMPENSATION)
+        .smartCurrentLimit(CURRENT_LIMIT);
+    topRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
+    topRightMotorConfig.apply(enc);
+
+    bottomLeftMotor.configure(
+        bottomLeftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    topLeftMotor.configure(
+        topLeftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    bottomRightMotor.configure(
+        bottomRightMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    topRightMotor.configure(
+        topRightMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+    topLeftMotorEncoder = topLeftMotor.getEncoder();
+    topRightMotorEncoder = topRightMotor.getEncoder();
+    bottomLeftMotorEncoder = bottomLeftMotor.getEncoder();
+    bottomRightMotorEncoder = bottomRightMotor.getEncoder();
   }
 
   @Override
   public void updateInputs(ShooterIOInputs inputs) {
-    inputs.middleMotorCurrentAmps = middleMotor.getOutputCurrent();
-    inputs.middleMotorAppliedVolts = middleMotor.getBusVoltage() * middleMotor.getAppliedOutput();
-    inputs.middleMotorVelocityRadPerSec = middleMotorEncoder.getVelocity();
-
-    inputs.bottomMotorCurrentAmps = bottomMotor.getOutputCurrent();
-    inputs.bottomMotorAppliedVolts = bottomMotor.getBusVoltage() * bottomMotor.getAppliedOutput();
-    inputs.bottomMotorVelocityRadPerSec = bottomMotorEncoder.getVelocity();
-
-    inputs.topMotorCurrentAmps = topMotor.getOutputCurrent();
-    inputs.topMotorAppliedVolts = topMotor.getBusVoltage() * topMotor.getAppliedOutput();
-    inputs.topMotorVelocityRadPerSec = topMotorEncoder.getVelocity();
+    inputs.topLeftMotorCurrentAmps = topLeftMotor.getOutputCurrent();
+    inputs.topLeftMotorAppliedVolts =
+        topLeftMotor.getBusVoltage() * topLeftMotor.getAppliedOutput();
+    inputs.topLeftMotorVelocityRadPerSec = topLeftMotorEncoder.getVelocity();
+
+    inputs.topRightMotorCurrentAmps = topRightMotor.getOutputCurrent();
+    inputs.topRightMotorAppliedVolts =
+        topRightMotor.getBusVoltage() * topRightMotor.getAppliedOutput();
+    inputs.topRightMotorVelocityRadPerSec = topRightMotorEncoder.getVelocity();
+
+    inputs.bottomLeftMotorCurrentAmps = bottomLeftMotor.getOutputCurrent();
+    inputs.bottomLeftMotorAppliedVolts =
+        bottomLeftMotor.getBusVoltage() * bottomLeftMotor.getAppliedOutput();
+    inputs.bottomLeftMotorVelocityRadPerSec = bottomLeftMotorEncoder.getVelocity();
+
+    inputs.bottomRightMotorCurrentAmps = bottomRightMotor.getOutputCurrent();
+    inputs.bottomRightMotorAppliedVolts =
+        bottomRightMotor.getBusVoltage() * bottomRightMotor.getAppliedOutput();
+    inputs.bottomRightMotorVelocityRadPerSec = bottomRightMotorEncoder.getVelocity();
 
     inputs.totalAmps =
-        inputs.middleMotorCurrentAmps + inputs.bottomMotorCurrentAmps + inputs.topMotorCurrentAmps;
+        inputs.topLeftMotorCurrentAmps
+            + inputs.topRightMotorCurrentAmps
+            + inputs.bottomLeftMotorCurrentAmps
+            + inputs.bottomRightMotorCurrentAmps;
 
     inputs.shooterWheelVelocityRadPerSec =
-        inputs.bottomMotorVelocityRadPerSec / SHOOTER_WHEEL_GEAR_RATIO;
-    inputs.shooterWheelPosition = bottomMotorEncoder.getPosition() / SHOOTER_WHEEL_GEAR_RATIO;
+        inputs.bottomLeftMotorVelocityRadPerSec / SHOOTER_WHEEL_GEAR_RATIO;
+    inputs.shooterWheelPosition = bottomLeftMotorEncoder.getPosition() / SHOOTER_WHEEL_GEAR_RATIO;
 
-    inputs.setpointRPM = bottomMotor.getClosedLoopController().getSetpoint();
-    inputs.atSetpoint = bottomMotor.getClosedLoopController().isAtSetpoint();
+    inputs.setpointRPM = bottomLeftMotor.getClosedLoopController().getSetpoint();
+    inputs.atSetpoint = bottomLeftMotor.getClosedLoopController().isAtSetpoint();
   }
 
   @Override
   public void setVoltage(double volts) {
-    bottomMotor.setVoltage(volts);
+    bottomLeftMotor.setVoltage(volts);
   }
 
   @Override
   public void stop() {
-    bottomMotor.set(0);
+    bottomLeftMotor.set(0);
   }
 
   private double distanceToRPM(double distanceMeters) {

From 69221d52be8ecc9a97416b6643ba8e8171d8f028 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Thu, 9 Apr 2026 18:51:42 -0400
Subject: [PATCH 014/102] Ishan is daddy and wrote this code because he is so
 smart

---
 src/main/java/frc/robot/RobotContainer.java   | 54 +++++++++++++++++++
 .../frc/robot/subsystems/shooter/Shooter.java | 24 +++++++++
 .../robot/subsystems/shooter/ShooterIO.java   |  8 +++
 .../subsystems/shooter/ShooterIOSparkMax.java | 35 ++++++++++--
 4 files changed, 118 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a42f0bbc1..63b12d96a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -47,6 +47,7 @@
 import frc.robot.subsystems.magicCarpet.MagicCarpetIO;
 import frc.robot.subsystems.magicCarpet.MagicCarpetSparkMax;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.subsystems.shooter.ShooterIO;
 import frc.robot.subsystems.shooter.ShooterIOSparkMax;
 import frc.robot.subsystems.vision.Vision;
@@ -149,6 +150,7 @@ public RobotContainer() {
           //          leds = new Leds();
           //    hopperBelt = new HopperBelt(new HopperBeltSparkMax());
           leds = new Leds();
+          shooter = new Shooter(new ShooterIOSparkMax(true));
         }
       }
     }
@@ -385,6 +387,58 @@ private void configureButtonBindings() {
                 () ->
                     operatorController.getLeftY()
                         * Units.rotationsPerMinuteToRadiansPerSecond(5600)));
+
+    if (Constants.getRobot() == Constants.RobotType.ROBOT_BRIEFCASE) {
+      // All 4 motors at max: A (forward) / back+A (reverse)
+      driverController
+          .a()
+          .and(() -> !driverController.back().getAsBoolean())
+          .whileTrue(shooter.setVoltage(ShooterConstants.MAX_VOLTAGE));
+      driverController
+          .a()
+          .and(driverController.back())
+          .whileTrue(shooter.setVoltage(-ShooterConstants.MAX_VOLTAGE));
+
+      // Bottom-left: B (forward) / back+B (reverse)
+      driverController
+          .b()
+          .and(() -> !driverController.back().getAsBoolean())
+          .whileTrue(shooter.setBottomLeftVoltage(ShooterConstants.MAX_VOLTAGE));
+      driverController
+          .b()
+          .and(driverController.back())
+          .whileTrue(shooter.setBottomLeftVoltage(-ShooterConstants.MAX_VOLTAGE));
+
+      // Top-left: X (forward) / back+X (reverse)
+      driverController
+          .x()
+          .and(() -> !driverController.back().getAsBoolean())
+          .whileTrue(shooter.setTopLeftVoltage(ShooterConstants.MAX_VOLTAGE));
+      driverController
+          .x()
+          .and(driverController.back())
+          .whileTrue(shooter.setTopLeftVoltage(-ShooterConstants.MAX_VOLTAGE));
+
+      // Top-right: Y (forward) / back+Y (reverse)
+      driverController
+          .y()
+          .and(() -> !driverController.back().getAsBoolean())
+          .whileTrue(shooter.setTopRightVoltage(ShooterConstants.MAX_VOLTAGE));
+      driverController
+          .y()
+          .and(driverController.back())
+          .whileTrue(shooter.setTopRightVoltage(-ShooterConstants.MAX_VOLTAGE));
+
+      // Bottom-right: left bumper (forward) / back+left bumper (reverse)
+      driverController
+          .leftBumper()
+          .and(() -> !driverController.back().getAsBoolean())
+          .whileTrue(shooter.setBottomRightVoltage(ShooterConstants.MAX_VOLTAGE));
+      driverController
+          .leftBumper()
+          .and(driverController.back())
+          .whileTrue(shooter.setBottomRightVoltage(-ShooterConstants.MAX_VOLTAGE));
+    }
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index b9aac7cf9..e9e0a469d 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -112,6 +112,30 @@ public Command setVoltage(double volts) {
         Commands.run(() -> io.setVoltage(volts), this));
   }
 
+  public Command setTopLeftVoltage(double volts) {
+    return Commands.sequence(
+        Commands.runOnce(() -> controllerEnabled = false, this),
+        Commands.run(() -> io.setTopLeftVoltage(volts), this));
+  }
+
+  public Command setTopRightVoltage(double volts) {
+    return Commands.sequence(
+        Commands.runOnce(() -> controllerEnabled = false, this),
+        Commands.run(() -> io.setTopRightVoltage(volts), this));
+  }
+
+  public Command setBottomLeftVoltage(double volts) {
+    return Commands.sequence(
+        Commands.runOnce(() -> controllerEnabled = false, this),
+        Commands.run(() -> io.setBottomLeftVoltage(volts), this));
+  }
+
+  public Command setBottomRightVoltage(double volts) {
+    return Commands.sequence(
+        Commands.runOnce(() -> controllerEnabled = false, this),
+        Commands.run(() -> io.setBottomRightVoltage(volts), this));
+  }
+
   public Command setTargetVelocityRPM(double rpm) {
     return Commands.run(
             () -> {
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
index 72a5219a9..612648e50 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
@@ -35,5 +35,13 @@ default void updateInputs(ShooterIOInputs inputs) {}
 
   default void setVoltage(double volts) {}
 
+  default void setTopLeftVoltage(double volts) {}
+
+  default void setTopRightVoltage(double volts) {}
+
+  default void setBottomLeftVoltage(double volts) {}
+
+  default void setBottomRightVoltage(double volts) {}
+
   default void stop() {}
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
index 2b562a2b0..1b9f090ec 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
@@ -24,8 +24,14 @@ public class ShooterIOSparkMax implements ShooterIO {
   private final RelativeEncoder topRightMotorEncoder;
   private final RelativeEncoder bottomLeftMotorEncoder;
   private final RelativeEncoder bottomRightMotorEncoder;
+  private final boolean independentMode;
 
   public ShooterIOSparkMax() {
+    this(false);
+  }
+
+  public ShooterIOSparkMax(boolean independentMode) {
+    this.independentMode = independentMode;
     topLeftMotor = new SparkMax(shooterTopLeftMotorCanId, MotorType.kBrushless);
     topRightMotor = new SparkMax(shooterTopRightMotorCanId, MotorType.kBrushless);
     bottomLeftMotor = new SparkMax(shooterBottomLeftMotorCanId, MotorType.kBrushless);
@@ -49,7 +55,7 @@ public ShooterIOSparkMax() {
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
-    topLeftMotorConfig.follow(bottomLeftMotor.getDeviceId(), false);
+    if (!independentMode) topLeftMotorConfig.follow(bottomLeftMotor.getDeviceId(), false);
     topLeftMotorConfig.apply(enc);
 
     var bottomRightMotorConfig = new SparkMaxConfig();
@@ -58,7 +64,7 @@ public ShooterIOSparkMax() {
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
-    bottomRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
+    if (!independentMode) bottomRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
     bottomRightMotorConfig.apply(enc);
 
     var topRightMotorConfig = new SparkMaxConfig();
@@ -67,7 +73,7 @@ public ShooterIOSparkMax() {
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
-    topRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
+    if (!independentMode) topRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
     topRightMotorConfig.apply(enc);
 
     bottomLeftMotor.configure(
@@ -126,9 +132,32 @@ public void setVoltage(double volts) {
     bottomLeftMotor.setVoltage(volts);
   }
 
+  @Override
+  public void setTopLeftVoltage(double volts) {
+    topLeftMotor.setVoltage(volts);
+  }
+
+  @Override
+  public void setTopRightVoltage(double volts) {
+    topRightMotor.setVoltage(volts);
+  }
+
+  @Override
+  public void setBottomLeftVoltage(double volts) {
+    bottomLeftMotor.setVoltage(volts);
+  }
+
+  @Override
+  public void setBottomRightVoltage(double volts) {
+    bottomRightMotor.setVoltage(volts);
+  }
+
   @Override
   public void stop() {
+    topLeftMotor.set(0);
+    topRightMotor.set(0);
     bottomLeftMotor.set(0);
+    bottomRightMotor.set(0);
   }
 
   private double distanceToRPM(double distanceMeters) {

From 4a402e3bee2acbd496c32cbc5dda1981833ff52f Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Fri, 10 Apr 2026 16:41:52 -0500
Subject: [PATCH 015/102] work on shooter

---
 src/main/java/frc/robot/RobotContainer.java   | 107 ++++--------------
 src/main/java/frc/robot/Schematic.java        |   8 +-
 .../frc/robot/subsystems/shooter/Shooter.java |  30 +----
 .../robot/subsystems/shooter/ShooterIO.java   |   3 -
 .../subsystems/shooter/ShooterIOSparkMax.java |  33 +-----
 5 files changed, 35 insertions(+), 146 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 63b12d96a..3518c1da7 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -27,7 +27,6 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.lib.utils.LocalADStarAK;
-import frc.robot.RobotState.IntakePosition;
 import frc.robot.commands.auto.Autos;
 import frc.robot.commands.auto.DriveToPose;
 import frc.robot.commands.drive.DriveCommands;
@@ -47,12 +46,10 @@
 import frc.robot.subsystems.magicCarpet.MagicCarpetIO;
 import frc.robot.subsystems.magicCarpet.MagicCarpetSparkMax;
 import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.subsystems.shooter.ShooterIO;
 import frc.robot.subsystems.shooter.ShooterIOSparkMax;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIOPhotonVision;
-import frc.robot.util.CustomTriggers;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -150,7 +147,7 @@ public RobotContainer() {
           //          leds = new Leds();
           //    hopperBelt = new HopperBelt(new HopperBeltSparkMax());
           leds = new Leds();
-          shooter = new Shooter(new ShooterIOSparkMax(true));
+          shooter = new Shooter(new ShooterIOSparkMax(false));
         }
       }
     }
@@ -335,28 +332,30 @@ private void configureButtonBindings() {
                 .ignoringDisable(true));
     new Trigger(() -> driverController.getHID().getPOV() != -1)
         .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV()));
-    driverController.x().toggleOnTrue(orchestrator.aimToHub());
-    driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
-    driverController
-        .leftBumper()
-        .and(operatorController.pov(180))
-        .whileTrue(intake.runIntakeExtendVolts(-4))
-        .onFalse(intake.stopExtendingCommand());
-    CustomTriggers.toggleIntakeUp(
-            driverController.leftBumper(),
-            () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED)
-        .and(() -> !operatorController.pov(180).getAsBoolean())
-        .toggleOnTrue(intake.extendToAngle(IntakeConstants.COLLAPSE_POS));
-    CustomTriggers.toggleIntakeDown(
-            driverController.leftBumper(),
-            () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED)
-        .and(() -> !operatorController.pov(180).getAsBoolean())
-        .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
+    //    driverController.x().toggleOnTrue(orchestrator.aimToHub());
+    //
+    // driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
+    //    driverController
+    //        .leftBumper()
+    //        .and(operatorController.pov(180))
+    //        .whileTrue(intake.runIntakeExtendVolts(-4))
+    //        .onFalse(intake.stopExtendingCommand());
+    //    CustomTriggers.toggleIntakeUp(
+    //            driverController.leftBumper(),
+    //            () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED)
+    //        .and(() -> !operatorController.pov(180).getAsBoolean())
+    //        .toggleOnTrue(intake.extendToAngle(IntakeConstants.COLLAPSE_POS));
+    //    CustomTriggers.toggleIntakeDown(
+    //            driverController.leftBumper(),
+    //            () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED)
+    //        .and(() -> !operatorController.pov(180).getAsBoolean())
+    //        .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
 
     // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
-    driverController.leftTrigger(0.2).toggleOnTrue(intake.intake());
+    //    driverController.leftTrigger(0.2).toggleOnTrue(intake.intake());
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
-    driverController.a().and(operatorController.pov(180)).onTrue(intake.resetExtendPosition());
+    //
+    // driverController.a().and(operatorController.pov(180)).onTrue(intake.resetExtendPosition());
     driverController
         .rightBumper()
         .and(() -> !operatorController.pov(180).getAsBoolean())
@@ -376,69 +375,9 @@ private void configureButtonBindings() {
         .rightTrigger(0.1)
         .and(operatorController.pov(0))
         .toggleOnTrue(orchestrator.spinUpShooterHub());
-    operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
+    operatorController.leftTrigger(0.1).toggleOnTrue(shooter.setVoltage(12));
     operatorController.y().whileTrue(indexer.reverse());
     //    operatorController.x().whileTrue(intake.outtake());
-
-    operatorController
-        .leftTrigger()
-        .whileTrue(
-            shooter.setTargetVelocityRadians(
-                () ->
-                    operatorController.getLeftY()
-                        * Units.rotationsPerMinuteToRadiansPerSecond(5600)));
-
-    if (Constants.getRobot() == Constants.RobotType.ROBOT_BRIEFCASE) {
-      // All 4 motors at max: A (forward) / back+A (reverse)
-      driverController
-          .a()
-          .and(() -> !driverController.back().getAsBoolean())
-          .whileTrue(shooter.setVoltage(ShooterConstants.MAX_VOLTAGE));
-      driverController
-          .a()
-          .and(driverController.back())
-          .whileTrue(shooter.setVoltage(-ShooterConstants.MAX_VOLTAGE));
-
-      // Bottom-left: B (forward) / back+B (reverse)
-      driverController
-          .b()
-          .and(() -> !driverController.back().getAsBoolean())
-          .whileTrue(shooter.setBottomLeftVoltage(ShooterConstants.MAX_VOLTAGE));
-      driverController
-          .b()
-          .and(driverController.back())
-          .whileTrue(shooter.setBottomLeftVoltage(-ShooterConstants.MAX_VOLTAGE));
-
-      // Top-left: X (forward) / back+X (reverse)
-      driverController
-          .x()
-          .and(() -> !driverController.back().getAsBoolean())
-          .whileTrue(shooter.setTopLeftVoltage(ShooterConstants.MAX_VOLTAGE));
-      driverController
-          .x()
-          .and(driverController.back())
-          .whileTrue(shooter.setTopLeftVoltage(-ShooterConstants.MAX_VOLTAGE));
-
-      // Top-right: Y (forward) / back+Y (reverse)
-      driverController
-          .y()
-          .and(() -> !driverController.back().getAsBoolean())
-          .whileTrue(shooter.setTopRightVoltage(ShooterConstants.MAX_VOLTAGE));
-      driverController
-          .y()
-          .and(driverController.back())
-          .whileTrue(shooter.setTopRightVoltage(-ShooterConstants.MAX_VOLTAGE));
-
-      // Bottom-right: left bumper (forward) / back+left bumper (reverse)
-      driverController
-          .leftBumper()
-          .and(() -> !driverController.back().getAsBoolean())
-          .whileTrue(shooter.setBottomRightVoltage(ShooterConstants.MAX_VOLTAGE));
-      driverController
-          .leftBumper()
-          .and(driverController.back())
-          .whileTrue(shooter.setBottomRightVoltage(-ShooterConstants.MAX_VOLTAGE));
-    }
   }
 
   /**
diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java
index 1065428f2..4e67e29d1 100644
--- a/src/main/java/frc/robot/Schematic.java
+++ b/src/main/java/frc/robot/Schematic.java
@@ -140,10 +140,10 @@ public class Schematic {
         backRightAbsoluteEncoderCanId = 0;
 
         // Shooter (CAN Ids)
-        shooterTopLeftMotorCanId = 0;
-        shooterTopRightMotorCanId = 0;
-        shooterBottomLeftMotorCanId = 0;
-        shooterBottomRightMotorCanId = 0;
+        shooterTopLeftMotorCanId = 8;
+        shooterTopRightMotorCanId = 1;
+        shooterBottomLeftMotorCanId = 7;
+        shooterBottomRightMotorCanId = 2;
 
         // Magic Carpet (CAN Ids)
         magicCarpetCanId = 0;
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index e9e0a469d..8ff49c4e8 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -75,6 +75,7 @@ public void periodic() {
 
     Logger.processInputs("Shooter", inputs);
     Logger.recordOutput("Shooter/AtSetpoint", isAtSetpoint());
+    RobotState.getInstance().shooterAtSpeed = isAtSetpoint();
   }
 
   public void runCharacterization(double voltage) {
@@ -107,33 +108,8 @@ public Command stop() {
   }
 
   public Command setVoltage(double volts) {
-    return Commands.sequence(
-        Commands.runOnce(() -> controllerEnabled = false, this),
-        Commands.run(() -> io.setVoltage(volts), this));
-  }
-
-  public Command setTopLeftVoltage(double volts) {
-    return Commands.sequence(
-        Commands.runOnce(() -> controllerEnabled = false, this),
-        Commands.run(() -> io.setTopLeftVoltage(volts), this));
-  }
-
-  public Command setTopRightVoltage(double volts) {
-    return Commands.sequence(
-        Commands.runOnce(() -> controllerEnabled = false, this),
-        Commands.run(() -> io.setTopRightVoltage(volts), this));
-  }
-
-  public Command setBottomLeftVoltage(double volts) {
-    return Commands.sequence(
-        Commands.runOnce(() -> controllerEnabled = false, this),
-        Commands.run(() -> io.setBottomLeftVoltage(volts), this));
-  }
-
-  public Command setBottomRightVoltage(double volts) {
-    return Commands.sequence(
-        Commands.runOnce(() -> controllerEnabled = false, this),
-        Commands.run(() -> io.setBottomRightVoltage(volts), this));
+    return Commands.runOnce(() -> controllerEnabled = false, this)
+        .andThen(Commands.run(() -> io.setVoltage(volts), this));
   }
 
   public Command setTargetVelocityRPM(double rpm) {
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
index 612648e50..06d151543 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
@@ -22,9 +22,6 @@ class ShooterIOInputs {
     public double bottomRightMotorAppliedVolts;
     public double bottomRightMotorCurrentAmps;
 
-    public boolean atSetpoint = false;
-    public double setpointRPM = 0;
-
     public double totalAmps;
 
     public double shooterWheelVelocityRadPerSec;
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
index 1b9f090ec..179a67f9b 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
@@ -51,11 +51,11 @@ public ShooterIOSparkMax(boolean independentMode) {
 
     var topLeftMotorConfig = new SparkMaxConfig();
     topLeftMotorConfig
-        .inverted(false)
+        .inverted(true)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
-    if (!independentMode) topLeftMotorConfig.follow(bottomLeftMotor.getDeviceId(), false);
+    topLeftMotorConfig.follow(bottomLeftMotor.getDeviceId(), false);
     topLeftMotorConfig.apply(enc);
 
     var bottomRightMotorConfig = new SparkMaxConfig();
@@ -64,16 +64,16 @@ public ShooterIOSparkMax(boolean independentMode) {
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
-    if (!independentMode) bottomRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
+    bottomRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
     bottomRightMotorConfig.apply(enc);
 
     var topRightMotorConfig = new SparkMaxConfig();
     topRightMotorConfig
-        .inverted(true)
+        .inverted(false)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
-    if (!independentMode) topRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
+    topRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
     topRightMotorConfig.apply(enc);
 
     bottomLeftMotor.configure(
@@ -122,9 +122,6 @@ public void updateInputs(ShooterIOInputs inputs) {
     inputs.shooterWheelVelocityRadPerSec =
         inputs.bottomLeftMotorVelocityRadPerSec / SHOOTER_WHEEL_GEAR_RATIO;
     inputs.shooterWheelPosition = bottomLeftMotorEncoder.getPosition() / SHOOTER_WHEEL_GEAR_RATIO;
-
-    inputs.setpointRPM = bottomLeftMotor.getClosedLoopController().getSetpoint();
-    inputs.atSetpoint = bottomLeftMotor.getClosedLoopController().isAtSetpoint();
   }
 
   @Override
@@ -132,26 +129,6 @@ public void setVoltage(double volts) {
     bottomLeftMotor.setVoltage(volts);
   }
 
-  @Override
-  public void setTopLeftVoltage(double volts) {
-    topLeftMotor.setVoltage(volts);
-  }
-
-  @Override
-  public void setTopRightVoltage(double volts) {
-    topRightMotor.setVoltage(volts);
-  }
-
-  @Override
-  public void setBottomLeftVoltage(double volts) {
-    bottomLeftMotor.setVoltage(volts);
-  }
-
-  @Override
-  public void setBottomRightVoltage(double volts) {
-    bottomRightMotor.setVoltage(volts);
-  }
-
   @Override
   public void stop() {
     topLeftMotor.set(0);

From 20c64f48b2edfa0527ff06e6a392cd46f3fe7a3f Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Fri, 10 Apr 2026 19:42:56 -0400
Subject: [PATCH 016/102] drive train changes

now align based off zones
---
 src/main/java/frc/robot/Constants.java        |   2 +-
 src/main/java/frc/robot/Orchestrator.java     | 119 ++++++++++++++++--
 src/main/java/frc/robot/RobotContainer.java   |   4 +-
 .../commands/auto/RotateToOrientation.java    |  28 ++++-
 .../subsystems/drive/DriveConstants.java      |   4 +-
 src/main/java/frc/robot/util/Zone.java        |  95 ++++++++++++++
 6 files changed, 229 insertions(+), 23 deletions(-)
 create mode 100644 src/main/java/frc/robot/util/Zone.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 922aca595..58fb36e35 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";
diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index e7702cde7..bcf09f239 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -22,10 +22,20 @@
 import frc.robot.subsystems.magicCarpet.MagicCarpet;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.util.ShooterLeadCompensator;
+import frc.robot.util.Zone;
+import frc.robot.util.Zone.Tuple2d;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 public class Orchestrator {
+  public enum ZoneId {
+    NONE,
+    ZONE_1,
+    ZONE_2,
+    ZONE_3,
+    ZONE_4
+  }
+
   private final double FRONT_HUB_OFFSET = Units.inchesToMeters(40.0);
   private final Drive drive;
   private final Shooter shooter;
@@ -45,6 +55,11 @@ public class Orchestrator {
   private final LinearFilter targetYFilter = LinearFilter.singlePoleIIR(0.06, 0.02);
   private final LinearFilter distanceFilter = LinearFilter.singlePoleIIR(0.06, 0.02);
 
+  private final Zone zone1;
+  private final Zone zone2;
+  private final Zone zone3;
+  private final Zone zone4;
+
   public Orchestrator(
       Drive drive,
       MagicCarpet hopperBelt,
@@ -59,6 +74,96 @@ public Orchestrator(
     this.intake = intake;
     this.shooterLeadCompensator = new ShooterLeadCompensator(drive, shooter);
     this.driverController = driverController;
+
+    this.zone1 = new Zone(drive::getPose);
+    this.zone2 = new Zone(drive::getPose);
+    this.zone3 = new Zone(drive::getPose);
+    this.zone4 = new Zone(drive::getPose);
+    zone1.initializeZone(new Tuple2d(0.0, 0.0), new Tuple2d(3.993527889251709, 8.100430488586426));
+    zone2.initializeZone(
+        new Tuple2d(4.307533264160156, 8.04629135131836), new Tuple2d(11.9086332321167, 0));
+    zone3.initializeZone(
+        new Tuple2d(13.69521713256836, 4.061668872833252),
+        new Tuple2d(16.51043891906738, 8.078774452209473));
+    zone4.initializeZone(
+        new Tuple2d(13.69521713256836, 4.061668872833252), new Tuple2d(16.564579010009766, 0));
+  }
+
+  public Zone getZone1() {
+    return zone1;
+  }
+
+  public Zone getZone2() {
+    return zone2;
+  }
+
+  public Zone getZone3() {
+    return zone3;
+  }
+
+  public Zone getZone4() {
+    return zone4;
+  }
+
+  public ZoneId getCurrentZone() {
+    if (zone1.isInZoneForAlliance()) {
+      return ZoneId.ZONE_1;
+    }
+    if (zone2.isInZoneForAlliance()) {
+      return ZoneId.ZONE_2;
+    }
+    if (zone3.isInZoneForAlliance()) {
+      return ZoneId.ZONE_3;
+    }
+    if (zone4.isInZoneForAlliance()) {
+      return ZoneId.ZONE_4;
+    }
+    return ZoneId.NONE;
+  }
+
+  public Command executeCurrentZoneLogic() {
+    double allianceY = AllianceFlipUtil.applyY(drive.getPose().getY());
+
+    return switch (getCurrentZone()) {
+      case ZONE_1 ->
+          new RotateToOrientation(
+              drive,
+              AllianceFlipUtil.apply(
+                  new Pose2d(4.621539115905762, 4.040013313293457, new Rotation2d())));
+      case ZONE_2 -> {
+        if (allianceY > 4.029185771942139) {
+          yield new RotateToOrientation(
+              drive,
+              AllianceFlipUtil.apply(
+                  new Pose2d(4.415811061859131, 5.599213600158691, new Rotation2d())));
+        } else {
+          yield new RotateToOrientation(
+              drive,
+              AllianceFlipUtil.apply(
+                  new Pose2d(4.415811061859131, 2.534952402114868, new Rotation2d())));
+        }
+      }
+      case ZONE_3 ->
+          new RotateToOrientation(
+              drive,
+              AllianceFlipUtil.apply(
+                  new Pose2d(11.334761619567871, 5.599213600158691, new Rotation2d())));
+      case ZONE_4 ->
+          new RotateToOrientation(
+              drive,
+              AllianceFlipUtil.apply(
+                  new Pose2d(11.334761619567871, 2.534952402114868, new Rotation2d())));
+      case NONE -> Commands.none();
+    };
+  }
+
+  public Command zoneBasedShoot() {
+    return Commands.deadline(
+            executeCurrentZoneLogic(),
+            indexer.run(),
+            Commands.run(
+                () -> Logger.recordOutput("Orchestrator/CurrentZone", getCurrentZone().name())))
+        .withName("zoneBasedShoot");
   }
 
   public Pose2d getShootWhileDrivingResultPose() {
@@ -97,6 +202,9 @@ private Rotation2d filteredHubAngle(Rotation2d raw) {
   }
 
   public void orchestratorPeriodic() {
+    Logger.recordOutput("Orchestrator/Pose", drive.getPose());
+    Logger.recordOutput("Orchestrator/CurrentZone", getCurrentZone().name());
+
     var shootWhileDrivingResult =
         shooterLeadCompensator.shootWhileDriving(
             AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()));
@@ -126,17 +234,6 @@ public Command driveToHub() {
         });
   }
 
-  public Command alignToHub() {
-    return new RotateToOrientation(
-        drive,
-        () -> {
-          Pose2d hubApproachPose = AllianceFlipUtil.apply(Hub.nearFace);
-          Rotation2d angle =
-              hubApproachPose.getTranslation().minus(drive.getPose().getTranslation()).getAngle();
-          return angle;
-        });
-  }
-
   public Command feedUp() {
     return Commands.repeatingSequence(
             Commands.parallel(magicCarpet.run(), indexer.run())
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a42f0bbc1..d6b913782 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -337,9 +337,7 @@ private void configureButtonBindings() {
     driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
     driverController
         .leftBumper()
-        .and(operatorController.pov(180))
-        .whileTrue(intake.runIntakeExtendVolts(-4))
-        .onFalse(intake.stopExtendingCommand());
+        .onTrue(Commands.runOnce(() -> orchestrator.executeCurrentZoneLogic().schedule()));
     CustomTriggers.toggleIntakeUp(
             driverController.leftBumper(),
             () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED)
diff --git a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java
index 48bcea7bc..9bdeec173 100644
--- a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java
+++ b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java
@@ -8,12 +8,13 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.drive.Drive;
+import java.util.Objects;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
 public class RotateToOrientation extends Command {
   private final Drive drive;
-  private final Supplier<Rotation2d> poseSupplier;
+  private final Supplier<Rotation2d> rotationSupplier;
   //  private final ProfiledPIDController driveController =
   //      new ProfiledPIDController(
   //          2.5, 0, 0.0, new Constraints(Units.inchesToMeters(150), Units.inchesToMeters(450.0)));
@@ -40,14 +41,28 @@ public RotateToOrientation(Drive drive, Rotation2d targetPose) {
     this(drive, () -> targetPose, DRIVE_TOLERANCE);
   }
 
-  public RotateToOrientation(Drive drive, Supplier<Rotation2d> targetPoseSupplier) {
-    this(drive, targetPoseSupplier, DRIVE_TOLERANCE);
+  public RotateToOrientation(Drive drive, Pose2d targetPose) {
+    this(
+        drive,
+        () -> targetPose.getTranslation().minus(drive.getPose().getTranslation()).getAngle(),
+        DRIVE_TOLERANCE);
+  }
+
+  public RotateToOrientation(Drive drive, Supplier<Pose2d> targetPoseSupplier) {
+    this(
+        drive,
+        () ->
+            Objects.requireNonNull(targetPoseSupplier.get(), "targetPoseSupplier.get()")
+                .getTranslation()
+                .minus(drive.getPose().getTranslation())
+                .getAngle(),
+        DRIVE_TOLERANCE);
   }
 
   public RotateToOrientation(
       Drive drive, Supplier<Rotation2d> targetPoseSupplier, double driveTolerance) {
     this.drive = drive;
-    this.poseSupplier = targetPoseSupplier;
+    this.rotationSupplier = Objects.requireNonNull(targetPoseSupplier, "targetPoseSupplier");
     addRequirements(drive);
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
   }
@@ -55,8 +70,8 @@ public RotateToOrientation(
   @Override
   public void initialize() {
     Pose2d currentPose = drive.getPose();
-    targetPose = poseSupplier.get();
-    Logger.recordOutput("StraightDriveToPose/TargetPose", targetPose);
+    targetPose = rotationSupplier.get();
+    Logger.recordOutput("RotateToOrientation/TargetPose", targetPose);
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
     thetaController.setTolerance(THETA_TOLERANCE);
     thetaController.reset(currentPose.getRotation().getRadians());
@@ -65,6 +80,7 @@ public void initialize() {
   @Override
   public void execute() {
     Pose2d currentPose = drive.getPose();
+    targetPose = rotationSupplier.get();
 
     // Calculate theta speed
     double thetaVelocity =
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index fe564acb2..d4d8b36e6 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -15,8 +15,8 @@
 public class DriveConstants {
   public static final double maxSpeedMetersPerSec = 11.8;
   public static final double odometryFrequency = 100.0; // Hz
-  public static final double trackWidth =0.5969;
-  public static final double wheelBase =0.5271;
+  public static final double trackWidth = 0.5969;
+  public static final double wheelBase = 0.5271;
   public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0);
   public static final Translation2d[] moduleTranslations =
       new Translation2d[] {
diff --git a/src/main/java/frc/robot/util/Zone.java b/src/main/java/frc/robot/util/Zone.java
new file mode 100644
index 000000000..3862daccd
--- /dev/null
+++ b/src/main/java/frc/robot/util/Zone.java
@@ -0,0 +1,95 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.lib.utils.AllianceFlipUtil;
+import java.util.Objects;
+import java.util.function.Supplier;
+
+public class Zone {
+  public record Tuple2d(double x, double y) {}
+
+  private Tuple2d minCorner;
+  private Tuple2d maxCorner;
+  private final Supplier<Pose2d> poseSupplier;
+
+  public Zone(Supplier<Pose2d> poseSupplier) {
+    this.poseSupplier = Objects.requireNonNull(poseSupplier, "poseSupplier");
+  }
+
+  public Zone(Supplier<Pose2d> poseSupplier, Tuple2d cornerA, Tuple2d cornerB) {
+    this(poseSupplier);
+    initializeZone(cornerA, cornerB);
+  }
+
+  public static Tuple2d corner(double x, double y) {
+    return new Tuple2d(x, y);
+  }
+
+  public void initializeZone(Tuple2d cornerA, Tuple2d cornerB) {
+    Objects.requireNonNull(cornerA, "cornerA");
+    Objects.requireNonNull(cornerB, "cornerB");
+
+    double minX = Math.min(cornerA.x(), cornerB.x());
+    double maxX = Math.max(cornerA.x(), cornerB.x());
+    double minY = Math.min(cornerA.y(), cornerB.y());
+    double maxY = Math.max(cornerA.y(), cornerB.y());
+
+    this.minCorner = new Tuple2d(minX, minY);
+    this.maxCorner = new Tuple2d(maxX, maxY);
+  }
+
+  public boolean isInitialized() {
+    return minCorner != null && maxCorner != null;
+  }
+
+  public boolean isInZone() {
+    return contains(poseSupplier.get());
+  }
+
+  public boolean isInZoneForAlliance() {
+    return containsForAlliance(poseSupplier.get());
+  }
+
+  public boolean contains(Pose2d pose) {
+    Objects.requireNonNull(pose, "pose");
+    return contains(pose.getX(), pose.getY());
+  }
+
+  public boolean containsForAlliance(Pose2d pose) {
+    Objects.requireNonNull(pose, "pose");
+    return containsForAlliance(pose.getX(), pose.getY());
+  }
+
+  public boolean contains(double x, double y) {
+    if (!isInitialized()) {
+      return false;
+    }
+
+    return x >= minCorner.x() && x <= maxCorner.x() && y >= minCorner.y() && y <= maxCorner.y();
+  }
+
+  public boolean containsForAlliance(double x, double y) {
+    if (!isInitialized()) {
+      return false;
+    }
+
+    double minX = minCorner.x();
+    double maxX = maxCorner.x();
+    double minY = minCorner.y();
+    double maxY = maxCorner.y();
+
+    if (AllianceFlipUtil.shouldFlip()) {
+      double flippedMinX = AllianceFlipUtil.applyX(minX);
+      double flippedMaxX = AllianceFlipUtil.applyX(maxX);
+      double flippedMinY = AllianceFlipUtil.applyY(minY);
+      double flippedMaxY = AllianceFlipUtil.applyY(maxY);
+
+      minX = Math.min(flippedMinX, flippedMaxX);
+      maxX = Math.max(flippedMinX, flippedMaxX);
+      minY = Math.min(flippedMinY, flippedMaxY);
+      maxY = Math.max(flippedMinY, flippedMaxY);
+    }
+
+    return x >= minX && x <= maxX && y >= minY && y <= maxY;
+  }
+}

From e84c28162e2b215d0a29cc0af20a635a916ae2a7 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Fri, 10 Apr 2026 18:40:44 -0500
Subject: [PATCH 017/102] updated shooter config

---
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../subsystems/shooter/ShooterConstants.java  |  4 ++--
 .../subsystems/shooter/ShooterIOSparkMax.java | 20 +++++++++----------
 3 files changed, 12 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3518c1da7..229e9a7c3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -375,7 +375,7 @@ private void configureButtonBindings() {
         .rightTrigger(0.1)
         .and(operatorController.pov(0))
         .toggleOnTrue(orchestrator.spinUpShooterHub());
-    operatorController.leftTrigger(0.1).toggleOnTrue(shooter.setVoltage(12));
+    operatorController.leftTrigger(0.1).toggleOnTrue(shooter.setVoltage(4));
     operatorController.y().whileTrue(indexer.reverse());
     //    operatorController.x().whileTrue(intake.outtake());
   }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index b5d07b404..03313f6eb 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -9,8 +9,8 @@ public class ShooterConstants {
   public static final double ENCODER_POSITION_CONVERSION = 1;
   public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60;
 
-  public static final double VOLTAGE_COMPENSATION = 12.0;
-  public static final int CURRENT_LIMIT = 20;
+  public static final double VOLTAGE_COMPENSATION = 9.0;
+  public static final int CURRENT_LIMIT = 40;
   public static final IdleMode IDLE_MODE = IdleMode.kCoast;
 
   public static final double KV = 0.0465;
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
index 179a67f9b..c8a389d04 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
@@ -24,19 +24,17 @@ public class ShooterIOSparkMax implements ShooterIO {
   private final RelativeEncoder topRightMotorEncoder;
   private final RelativeEncoder bottomLeftMotorEncoder;
   private final RelativeEncoder bottomRightMotorEncoder;
-  private final boolean independentMode;
 
   public ShooterIOSparkMax() {
     this(false);
   }
 
   public ShooterIOSparkMax(boolean independentMode) {
-    this.independentMode = independentMode;
     topLeftMotor = new SparkMax(shooterTopLeftMotorCanId, MotorType.kBrushless);
     topRightMotor = new SparkMax(shooterTopRightMotorCanId, MotorType.kBrushless);
     bottomLeftMotor = new SparkMax(shooterBottomLeftMotorCanId, MotorType.kBrushless);
     bottomRightMotor = new SparkMax(shooterBottomRightMotorCanId, MotorType.kBrushless);
-
+    //
     EncoderConfig enc = new EncoderConfig();
     enc.positionConversionFactor(ENCODER_POSITION_CONVERSION);
     enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION);
@@ -47,24 +45,25 @@ public ShooterIOSparkMax(boolean independentMode) {
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
+    bottomLeftMotorConfig.follow(shooterTopRightMotorCanId, false);
     bottomLeftMotorConfig.apply(enc);
 
     var topLeftMotorConfig = new SparkMaxConfig();
     topLeftMotorConfig
-        .inverted(true)
+        .inverted(false)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
-    topLeftMotorConfig.follow(bottomLeftMotor.getDeviceId(), false);
+    topLeftMotorConfig.follow(shooterTopRightMotorCanId, true);
     topLeftMotorConfig.apply(enc);
 
     var bottomRightMotorConfig = new SparkMaxConfig();
     bottomRightMotorConfig
-        .inverted(true)
+        .inverted(false)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
-    bottomRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
+    bottomRightMotorConfig.follow(shooterTopRightMotorCanId, true);
     bottomRightMotorConfig.apply(enc);
 
     var topRightMotorConfig = new SparkMaxConfig();
@@ -73,7 +72,6 @@ public ShooterIOSparkMax(boolean independentMode) {
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
         .smartCurrentLimit(CURRENT_LIMIT);
-    topRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true);
     topRightMotorConfig.apply(enc);
 
     bottomLeftMotor.configure(
@@ -120,13 +118,13 @@ public void updateInputs(ShooterIOInputs inputs) {
             + inputs.bottomRightMotorCurrentAmps;
 
     inputs.shooterWheelVelocityRadPerSec =
-        inputs.bottomLeftMotorVelocityRadPerSec / SHOOTER_WHEEL_GEAR_RATIO;
-    inputs.shooterWheelPosition = bottomLeftMotorEncoder.getPosition() / SHOOTER_WHEEL_GEAR_RATIO;
+        inputs.topRightMotorVelocityRadPerSec / SHOOTER_WHEEL_GEAR_RATIO;
+    inputs.shooterWheelPosition = topRightMotorEncoder.getPosition() / SHOOTER_WHEEL_GEAR_RATIO;
   }
 
   @Override
   public void setVoltage(double volts) {
-    bottomLeftMotor.setVoltage(volts);
+    topRightMotor.setVoltage(volts);
   }
 
   @Override

From d58a3a9d9ade52d9c8b0de09fe41545c65a524c7 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Fri, 10 Apr 2026 18:55:03 -0500
Subject: [PATCH 018/102] add in removed robotContainer button bindings

---
 src/main/java/frc/robot/RobotContainer.java | 46 +++++++++++----------
 1 file changed, 24 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 229e9a7c3..6c9f61ed0 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -27,6 +27,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.lib.utils.LocalADStarAK;
+import frc.robot.RobotState.IntakePosition;
 import frc.robot.commands.auto.Autos;
 import frc.robot.commands.auto.DriveToPose;
 import frc.robot.commands.drive.DriveCommands;
@@ -50,6 +51,7 @@
 import frc.robot.subsystems.shooter.ShooterIOSparkMax;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIOPhotonVision;
+import frc.robot.util.CustomTriggers;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -332,30 +334,30 @@ private void configureButtonBindings() {
                 .ignoringDisable(true));
     new Trigger(() -> driverController.getHID().getPOV() != -1)
         .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV()));
-    //    driverController.x().toggleOnTrue(orchestrator.aimToHub());
-    //
-    // driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
-    //    driverController
-    //        .leftBumper()
-    //        .and(operatorController.pov(180))
-    //        .whileTrue(intake.runIntakeExtendVolts(-4))
-    //        .onFalse(intake.stopExtendingCommand());
-    //    CustomTriggers.toggleIntakeUp(
-    //            driverController.leftBumper(),
-    //            () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED)
-    //        .and(() -> !operatorController.pov(180).getAsBoolean())
-    //        .toggleOnTrue(intake.extendToAngle(IntakeConstants.COLLAPSE_POS));
-    //    CustomTriggers.toggleIntakeDown(
-    //            driverController.leftBumper(),
-    //            () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED)
-    //        .and(() -> !operatorController.pov(180).getAsBoolean())
-    //        .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
+    driverController.x().toggleOnTrue(orchestrator.aimToHub());
 
-    // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
-    //    driverController.leftTrigger(0.2).toggleOnTrue(intake.intake());
+    driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
+    driverController
+        .leftBumper()
+        .and(operatorController.pov(180))
+        .whileTrue(intake.runIntakeExtendVolts(-4))
+        .onFalse(intake.stopExtendingCommand());
+    CustomTriggers.toggleIntakeUp(
+            driverController.leftBumper(),
+            () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED)
+        .and(() -> !operatorController.pov(180).getAsBoolean())
+        .toggleOnTrue(intake.extendToAngle(IntakeConstants.COLLAPSE_POS));
+    CustomTriggers.toggleIntakeDown(
+            driverController.leftBumper(),
+            () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED)
+        .and(() -> !operatorController.pov(180).getAsBoolean())
+        .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
+
+    //     VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
+    driverController.leftTrigger(0.2).toggleOnTrue(intake.intake());
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
-    //
-    // driverController.a().and(operatorController.pov(180)).onTrue(intake.resetExtendPosition());
+
+    driverController.a().and(operatorController.pov(180)).onTrue(intake.resetExtendPosition());
     driverController
         .rightBumper()
         .and(() -> !operatorController.pov(180).getAsBoolean())

From ef3f4de22d14b5b7c0d9bd7d7cf62ac4f63e1e7f Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Fri, 10 Apr 2026 21:05:40 -0400
Subject: [PATCH 019/102] update magic carpet

---
 .../frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java  | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
index c318f0ee9..51890b777 100644
--- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
+++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
@@ -5,7 +5,7 @@ public final class MagicCarpetConstants {
   public static final boolean MOTOR_INVERTED = true;
 
   public static final int CURRENT_LIMIT = 30;
-  public static final double BELT_SPEED = 0.4;
+  public static final double BELT_SPEED = 0.8; //TODO: bump this value up if needed
 
   private MagicCarpetConstants() {}
   ;

From 54f38047ed0c54c58acb43bc3d63f0b0a155c6b6 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Fri, 10 Apr 2026 21:18:57 -0400
Subject: [PATCH 020/102] build

---
 .../frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java  | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
index 51890b777..8980bcc33 100644
--- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
+++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
@@ -5,7 +5,7 @@ public final class MagicCarpetConstants {
   public static final boolean MOTOR_INVERTED = true;
 
   public static final int CURRENT_LIMIT = 30;
-  public static final double BELT_SPEED = 0.8; //TODO: bump this value up if needed
+  public static final double BELT_SPEED = 0.8; // TODO: bump this value up if needed
 
   private MagicCarpetConstants() {}
   ;

From 3af4f7c620c2c328b3471f4e69ccd708402260f6 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Fri, 10 Apr 2026 21:24:49 -0400
Subject: [PATCH 021/102] setting changes

---
 .../deploy/pathplanner/paths/A-Cycle1.path    | 153 ++++++++++--------
 .../deploy/pathplanner/paths/B-Cycle1.path    | 119 +++++++++-----
 src/main/deploy/pathplanner/settings.json     |   4 +-
 3 files changed, 171 insertions(+), 105 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index 047a32cfd..cc35c91cd 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -3,143 +3,168 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.6619856887298745,
-        "y": 7.35309481216458
+        "x": 4.294776386404293,
+        "y": 7.530574239713776
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.023996536488359,
-        "y": 7.372382174256761
-      },
-      "isLocked": false,
-      "linkedName": "A-trench"
-    },
-    {
-      "anchor": {
-        "x": 5.398103756708408,
-        "y": 7.35309481216458
-      },
-      "prevControl": {
-        "x": 4.853359758746704,
-        "y": 7.4333678569314285
-      },
-      "nextControl": {
-        "x": 6.13088641115691,
-        "y": 7.24511253967509
+        "x": 5.22064231261789,
+        "y": 7.586659838431375
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.963113468715122,
-        "y": 6.645209129630746
+        "x": 7.588533094812165,
+        "y": 6.832881932021468
       },
       "prevControl": {
-        "x": 7.701961706280031,
-        "y": 6.985217185332927
+        "x": 7.335202354903501,
+        "y": 7.21155347245828
       },
       "nextControl": {
-        "x": 8.191055623727381,
-        "y": 6.348438515426881
+        "x": 7.834164231584512,
+        "y": 6.465719538012889
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.7068785174481285,
-        "y": 4.5489918830178
+        "x": 7.7021109123434695,
+        "y": 4.074563506261182
       },
       "prevControl": {
-        "x": 8.128187503978547,
-        "y": 4.672968428198327
+        "x": 8.22271615942871,
+        "y": 4.362336125245133
       },
       "nextControl": {
-        "x": 7.089374736141768,
-        "y": 4.367282050353818
+        "x": 7.200064179283715,
+        "y": 3.7970493935359855
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.690161001788908,
-        "y": 7.35309481216458
+        "x": 5.965992844364937,
+        "y": 5.664652951699464
       },
       "prevControl": {
-        "x": 6.265694051261446,
-        "y": 6.928023886217352
+        "x": 6.215916040758477,
+        "y": 5.658456508813674
       },
       "nextControl": {
-        "x": 5.489063021944757,
-        "y": 7.5016195660024545
+        "x": 4.002719141323792,
+        "y": 5.713329159212882
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.3699284436493735,
-        "y": 7.223291592128801
+        "x": 3.256350626118068,
+        "y": 5.664652951699464
       },
       "prevControl": {
-        "x": 3.929443246590092,
-        "y": 7.438867658239059
+        "x": 4.3759033989266545,
+        "y": 5.729554561717354
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "A-Cycle1-End"
     }
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.7620320855614973,
+      "waypointRelativePos": 0.29946524064171126,
       "rotationDegrees": 0.0
     },
     {
-      "waypointRelativePos": 1.9331550802139064,
-      "rotationDegrees": -106.336
+      "waypointRelativePos": 1.171122994652407,
+      "rotationDegrees": -93.685
+    },
+    {
+      "waypointRelativePos": 1.9411764705882377,
+      "rotationDegrees": -136.0
+    },
+    {
+      "waypointRelativePos": 3.0160427807486627,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 3.8502673796791456,
+      "rotationDegrees": 0.0
     }
   ],
-  "constraintZones": [],
-  "pointTowardsZones": [
+  "constraintZones": [
     {
-      "fieldPosition": {
-        "x": 4.585,
-        "y": 4.005
-      },
-      "rotationOffset": 0.0,
-      "minWaypointRelativePos": 4.3559322033898304,
-      "maxWaypointRelativePos": 5.0,
-      "name": "Hub"
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.1254237288135598,
+      "maxWaypointRelativePos": 2.1694915254237293,
+      "constraints": {
+        "maxVelocity": 1.2,
+        "maxAcceleration": 6,
+        "maxAngularVelocity": 540,
+        "maxAngularAcceleration": 720,
+        "nominalVoltage": 12,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 3.118644067796608,
+      "maxWaypointRelativePos": 3.5254237288135593,
+      "constraints": {
+        "maxVelocity": 1.2,
+        "maxAcceleration": 6,
+        "maxAngularVelocity": 540,
+        "maxAngularAcceleration": 720,
+        "nominalVoltage": 12,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 0,
+      "constraints": {
+        "maxVelocity": 4,
+        "maxAcceleration": 6,
+        "maxAngularVelocity": 540,
+        "maxAngularAcceleration": 720,
+        "nominalVoltage": 12,
+        "unlimited": false
+      }
     }
   ],
+  "pointTowardsZones": [],
   "eventMarkers": [
     {
       "name": "startIntake",
-      "waypointRelativePos": 0.7423728813559323,
+      "waypointRelativePos": 0.5288135593220339,
       "endWaypointRelativePos": null,
       "command": null
     },
     {
       "name": "preloadBalls",
-      "waypointRelativePos": 3.779661016949158,
+      "waypointRelativePos": 2.535593220338976,
       "endWaypointRelativePos": null,
       "command": null
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "maxVelocity": 4,
+    "maxAcceleration": 6,
+    "maxAngularVelocity": 540,
+    "maxAngularAcceleration": 720,
+    "nominalVoltage": 12,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -68.87528085392752
+    "rotation": -51.09865544424842
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index d187d214b..b508525b9 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -3,80 +3,84 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.6457602862254017,
-        "y": 0.603327370304114
+        "x": 4.294776386404293,
+        "y": 0.5384257602862248
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.645760286225405,
-        "y": 0.603327370304114
+        "x": 5.22064231261789,
+        "y": 0.48234016156862536
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.343222070669198,
-        "y": 1.2331899073216823
+        "x": 7.588533094812165,
+        "y": 1.236118067978532
       },
       "prevControl": {
-        "x": 6.726656775499253,
-        "y": 0.6653008196651526
+        "x": 7.335202354903501,
+        "y": 0.8574465275417207
       },
       "nextControl": {
-        "x": 7.9597873658391425,
-        "y": 1.801078994978212
+        "x": 7.834164231584512,
+        "y": 1.603280461987111
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.70378850168996,
-        "y": 3.5794281843111113
+        "x": 7.7021109123434695,
+        "y": 3.994436493738819
       },
       "prevControl": {
-        "x": 8.239226784337545,
-        "y": 3.49830117178875
+        "x": 8.22271615942871,
+        "y": 3.7066638747548675
       },
       "nextControl": {
-        "x": 7.168350219042376,
-        "y": 3.6605551968334726
+        "x": 7.200064179283715,
+        "y": 4.271950606464015
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.112021466905188,
-        "y": 1.5281753130590334
+        "x": 5.965992844364937,
+        "y": 2.4043470483005365
       },
       "prevControl": {
-        "x": 6.092572868677908,
-        "y": 1.7810070900136763
+        "x": 6.215916040758477,
+        "y": 2.410543491186327
       },
       "nextControl": {
-        "x": 6.225599284436494,
-        "y": 0.05166368515205688
+        "x": 4.002719141323792,
+        "y": 2.355670840787119
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.3212522361359564,
-        "y": 0.7980322003577818
+        "x": 3.256350626118068,
+        "y": 2.4043470483005365
       },
       "prevControl": {
-        "x": 3.5677075285819884,
-        "y": 0.7560823633456908
+        "x": 4.3759033989266545,
+        "y": 2.339445438282647
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "B-Cycle1-End"
     }
   ],
   "rotationTargets": [
+    {
+      "waypointRelativePos": 0.29946524064171126,
+      "rotationDegrees": 0.0
+    },
     {
       "waypointRelativePos": 1.171122994652407,
       "rotationDegrees": 86.315500893566
@@ -84,21 +88,58 @@
     {
       "waypointRelativePos": 1.9411764705882377,
       "rotationDegrees": 115.5063543100694
+    },
+    {
+      "waypointRelativePos": 3.0160427807486627,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 3.8502673796791456,
+      "rotationDegrees": 0.0
     }
   ],
-  "constraintZones": [],
-  "pointTowardsZones": [
+  "constraintZones": [
     {
-      "fieldPosition": {
-        "x": 4.615,
-        "y": 4.045
-      },
-      "rotationOffset": 0.0,
-      "minWaypointRelativePos": 3.620338983050851,
-      "maxWaypointRelativePos": 4.0,
-      "name": "Hub"
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.1254237288135598,
+      "maxWaypointRelativePos": 2.1694915254237293,
+      "constraints": {
+        "maxVelocity": 1.2,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 3.118644067796608,
+      "maxWaypointRelativePos": 3.5254237288135593,
+      "constraints": {
+        "maxVelocity": 1.2,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 0,
+      "constraints": {
+        "maxVelocity": 4.0,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
     }
   ],
+  "pointTowardsZones": [],
   "eventMarkers": [
     {
       "name": "startIntake",
@@ -114,7 +155,7 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.0,
+    "maxVelocity": 4.0,
     "maxAcceleration": 6.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -123,7 +164,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 67.83365417791755
+    "rotation": 50.71059313749959
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 9134dcf0b..43ec2a33c 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -4,7 +4,7 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 2.0,
+  "defaultMaxVel": 4.0,
   "defaultMaxAccel": 6.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
@@ -29,4 +29,4 @@
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
-}
+}
\ No newline at end of file

From b1180cee7a06c125d385c828cfdbd3343538b8b5 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Fri, 10 Apr 2026 20:27:33 -0500
Subject: [PATCH 022/102] change turn motor reductions for opus

---
 src/main/java/frc/robot/subsystems/drive/DriveConstants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index d4d8b36e6..0d372bae3 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -84,7 +84,7 @@ public class DriveConstants {
   // Turn motor configuration
   public static final boolean turnInverted = false;
   public static final int turnMotorCurrentLimit = 10;
-  public static final double turnMotorReduction = 12.8;
+  public static final double turnMotorReduction = (double) 287 / 11;
   public static final DCMotor turnGearbox = DCMotor.getNEO(1);
 
   // Turn encoder configuration

From 8cf4e31d6a2026795678689492ed3da7bf12bda0 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Fri, 10 Apr 2026 21:34:13 -0400
Subject: [PATCH 023/102] update intake

---
 src/main/java/frc/robot/Orchestrator.java     |   4 +
 src/main/java/frc/robot/RobotContainer.java   |  54 ++--
 .../java/frc/robot/commands/auto/Autos.java   |  30 ++-
 .../frc/robot/subsystems/intake/Intake.java   | 249 ++----------------
 .../subsystems/intake/IntakeConstants.java    |   3 +-
 .../frc/robot/subsystems/intake/IntakeIO.java |  54 ----
 .../subsystems/intake/IntakeIOKraken.java     | 155 -----------
 .../subsystems/intake/IntakeIOSparkMax.java   | 136 ----------
 8 files changed, 78 insertions(+), 607 deletions(-)
 delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIO.java
 delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOKraken.java
 delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index e7702cde7..e84427f8b 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -19,6 +19,7 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.rollers.IntakeRollers;
 import frc.robot.subsystems.magicCarpet.MagicCarpet;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.util.ShooterLeadCompensator;
@@ -32,6 +33,7 @@ public class Orchestrator {
   private final MagicCarpet magicCarpet;
   private final Indexer indexer;
   private final Intake intake;
+  private final IntakeRollers rollers;
   private final ShooterLeadCompensator shooterLeadCompensator;
   private final CommandXboxController driverController;
 
@@ -51,12 +53,14 @@ public Orchestrator(
       Shooter shooter,
       Indexer indexer,
       Intake intake,
+      IntakeRollers rollers,
       CommandXboxController driverController) {
     this.drive = drive;
     this.magicCarpet = hopperBelt;
     this.shooter = shooter;
     this.indexer = indexer;
     this.intake = intake;
+    this.rollers = rollers;
     this.shooterLeadCompensator = new ShooterLeadCompensator(drive, shooter);
     this.driverController = driverController;
   }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 6c9f61ed0..956728d57 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -40,8 +40,12 @@
 import frc.robot.subsystems.indexer.IndexerIOSparkMax;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.intake.IntakeConstants;
-import frc.robot.subsystems.intake.IntakeIO;
-import frc.robot.subsystems.intake.IntakeIOKraken;
+import frc.robot.subsystems.intake.extend.IntakeExtend;
+import frc.robot.subsystems.intake.extend.IntakeExtendIO;
+import frc.robot.subsystems.intake.extend.IntakeExtendIOSparkMax;
+import frc.robot.subsystems.intake.rollers.IntakeRollers;
+import frc.robot.subsystems.intake.rollers.IntakeRollersIO;
+import frc.robot.subsystems.intake.rollers.IntakeRollersIOKraken;
 import frc.robot.subsystems.leds.Leds;
 import frc.robot.subsystems.magicCarpet.MagicCarpet;
 import frc.robot.subsystems.magicCarpet.MagicCarpetIO;
@@ -73,6 +77,8 @@ public class RobotContainer {
   private Shooter shooter;
   private Climber climber;
   private Intake intake;
+  private IntakeRollers intakeRollers;
+  private IntakeExtend intakeExtend;
   private RobotState robotState = RobotState.getInstance();
   private boolean isRobotOriented = true; // Workaround, change if needed
 
@@ -124,12 +130,9 @@ public RobotContainer() {
           shooter = new Shooter(new ShooterIOSparkMax());
           magicCarpet = new MagicCarpet(new MagicCarpetSparkMax());
           indexer = new Indexer(new IndexerIOSparkMax());
-          // TODO: GET THE ACTUAL BUTTON BINDINGS FOR THE OP SWITCHES
-          intake =
-              new Intake(
-                  new IntakeIOKraken(),
-                  operatorController.rightTrigger(),
-                  operatorController.leftBumper());
+          intakeRollers = new IntakeRollers(new IntakeRollersIOKraken());
+          intakeExtend =
+              new IntakeExtend(new IntakeExtendIOSparkMax(), operatorController.rightTrigger());
           //                    climber = new Climber(new ClimberIOPhysical());
         }
 
@@ -164,9 +167,13 @@ public RobotContainer() {
               new ModuleIO() {},
               new ModuleIO() {});
     }
-    if (intake == null) {
-      intake = new Intake(new IntakeIO() {}, () -> false, () -> false);
+    if (intakeRollers == null) {
+      intakeRollers = new IntakeRollers(new IntakeRollersIO() {});
     }
+    if (intakeExtend == null) {
+      intakeExtend = new IntakeExtend(new IntakeExtendIO() {}, () -> false);
+    }
+    intake = new Intake(intakeRollers, intakeExtend);
     if (magicCarpet == null) {
       magicCarpet = new MagicCarpet(new MagicCarpetIO() {});
     }
@@ -180,14 +187,16 @@ public RobotContainer() {
       climber = new Climber(new ClimberIO() {});
     }
 
-    orchestrator = new Orchestrator(drive, magicCarpet, shooter, indexer, intake, driverController);
-    Autos autos = new Autos(drive, orchestrator, intake, shooter);
+    orchestrator =
+        new Orchestrator(
+            drive, magicCarpet, shooter, indexer, intake, intakeRollers, driverController);
+    Autos autos = new Autos(drive, orchestrator, intake, intakeRollers, shooter);
     NamedCommands.registerCommand(
         "startIntake", intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(10.0));
     NamedCommands.registerCommand(
         "endIntake",
         Commands.parallel(
-                intake.stopIntakeCommand().withTimeout(0.05),
+                intakeRollers.stopIntakeCommand().withTimeout(0.05),
                 magicCarpet.stop().withTimeout(0.05),
                 indexer.stop().withTimeout(0.05))
             .withTimeout(0.05));
@@ -207,7 +216,7 @@ public RobotContainer() {
                         .setTargetVelocityRadiansRepeatedly(
                             Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM))
                         .withTimeout(0.8),
-                    intake.stopIntakeCommand(),
+                    intakeRollers.stopIntakeCommand(),
                     magicCarpet.stop(),
                     indexer.stop())
                 .withTimeout(0.8),
@@ -340,13 +349,13 @@ private void configureButtonBindings() {
     driverController
         .leftBumper()
         .and(operatorController.pov(180))
-        .whileTrue(intake.runIntakeExtendVolts(-4))
-        .onFalse(intake.stopExtendingCommand());
+        .whileTrue(intakeExtend.runIntakeExtendVolts(-4))
+        .onFalse(intakeExtend.stopExtendingCommand());
     CustomTriggers.toggleIntakeUp(
             driverController.leftBumper(),
             () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED)
         .and(() -> !operatorController.pov(180).getAsBoolean())
-        .toggleOnTrue(intake.extendToAngle(IntakeConstants.COLLAPSE_POS));
+        .toggleOnTrue(intakeExtend.extendToAngle(IntakeConstants.COLLAPSE_POS));
     CustomTriggers.toggleIntakeDown(
             driverController.leftBumper(),
             () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED)
@@ -354,10 +363,13 @@ private void configureButtonBindings() {
         .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
 
     //     VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
-    driverController.leftTrigger(0.2).toggleOnTrue(intake.intake());
+    driverController.leftTrigger(0.2).toggleOnTrue(intakeRollers.intake());
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
 
-    driverController.a().and(operatorController.pov(180)).onTrue(intake.resetExtendPosition());
+    driverController
+        .a()
+        .and(operatorController.pov(180))
+        .onTrue(intakeExtend.resetExtendPosition());
     driverController
         .rightBumper()
         .and(() -> !operatorController.pov(180).getAsBoolean())
@@ -365,8 +377,8 @@ private void configureButtonBindings() {
     driverController
         .rightBumper()
         .and(operatorController.pov(180))
-        .whileTrue(intake.runIntakeExtendVolts(4))
-        .onFalse(intake.stopExtendingCommand());
+        .whileTrue(intakeExtend.runIntakeExtendVolts(4))
+        .onFalse(intakeExtend.stopExtendingCommand());
     //    operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
     operatorController
         .rightTrigger(0.1)
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 5e8a1925b..abe373e4f 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -10,6 +10,7 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.rollers.IntakeRollers;
 import frc.robot.subsystems.shooter.Shooter;
 import java.util.function.Supplier;
 
@@ -17,12 +18,19 @@ public class Autos {
   private final Drive drive;
   private final Orchestrator orchestrator;
   private final Intake intake;
+  private final IntakeRollers rollers;
   private final Shooter shooter;
 
-  public Autos(Drive drive, Orchestrator orchestrator, Intake intake, Shooter shooter) {
+  public Autos(
+      Drive drive,
+      Orchestrator orchestrator,
+      Intake intake,
+      IntakeRollers rollers,
+      Shooter shooter) {
     this.drive = drive;
     this.orchestrator = orchestrator;
     this.intake = intake;
+    this.rollers = rollers;
     this.shooter = shooter;
   }
 
@@ -104,7 +112,7 @@ public Command ACCManuelAuto() {
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5),
             Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseA.get())).withTimeout(5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
         Commands.parallel(
@@ -120,7 +128,7 @@ public Command ADepot() {
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPose.get())).withTimeout(3.5),
             Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get()))
                 .withTimeout(2),
@@ -130,7 +138,7 @@ public Command ADepot() {
             orchestrator.spinUpShooter(1250),
             orchestrator.feedUp(),
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())).withTimeout(2),
         Commands.deadline(
@@ -145,7 +153,7 @@ public Command ACCManuelAutoAlt() {
             Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get()))
             .withTimeout(3.5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get()))
                 .withTimeout(2),
@@ -163,7 +171,7 @@ public Command ACCManuelAutoOverBump() {
             Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get()))
             .withTimeout(3.5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get()))
                 .withTimeout(2),
@@ -173,7 +181,7 @@ public Command ACCManuelAutoOverBump() {
             orchestrator.spinUpShooter(1250),
             orchestrator.feedUp(),
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())).withTimeout(2),
         Commands.deadline(new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())))
@@ -187,7 +195,7 @@ public Command BCCManuelAuto() {
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5),
             Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseB.get())).withTimeout(5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
         Commands.parallel(
@@ -203,7 +211,7 @@ public Command BCCManuelAutoAlt() {
             Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get()))
             .withTimeout(3.5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt2.get()))
                 .withTimeout(2),
@@ -221,7 +229,7 @@ public Command BCCManuelAutoOverBump() {
             Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get()))
             .withTimeout(3.5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt2.get()))
                 .withTimeout(2),
@@ -231,7 +239,7 @@ public Command BCCManuelAutoOverBump() {
             orchestrator.spinUpShooter(1250),
             orchestrator.feedUp(),
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get())).withTimeout(2),
         Commands.deadline(
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 7ce9bc9a2..d89a8c53c 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -2,254 +2,45 @@
 
 import static frc.robot.subsystems.intake.IntakeConstants.*;
 
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.RobotState;
-import frc.robot.RobotState.IntakePosition;
-import java.util.function.BooleanSupplier;
-import org.littletonrobotics.junction.Logger;
+import frc.robot.subsystems.intake.extend.IntakeExtend;
+import frc.robot.subsystems.intake.rollers.IntakeRollers;
 
-public class Intake extends SubsystemBase {
-  private final Timer stallExtendTimer = new Timer();
-  private final Timer stallCollapseTimer = new Timer();
+// TODO:rewrite this for v1.5
+public class Intake {
+  private final IntakeRollers rollers;
+  private final IntakeExtend extend;
 
-  private boolean stalledExtend = false;
-  private boolean stalledCollapse = false;
-  private boolean isStowed = false;
-  private boolean isExtended = false;
-
-  private double targetExtendPosition = 0;
-
-  private final IntakeIO io;
-  private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
-  private final BooleanSupplier limitSwitchDisabled;
-  private final BooleanSupplier pidModeDisabled;
-
-  /**
-   * Constructor for the Intake subsystem
-   *
-   * @param io The IO implementation to use
-   * @param limitSwitchDisabled A supplier to return whether the limit switch is currently disabled
-   *     or not. If disabled, uses slipping to control intake.
-   */
-  public Intake(IntakeIO io, BooleanSupplier limitSwitchDisabled, BooleanSupplier pidModeDisabled) {
-    this.io = io;
-    this.limitSwitchDisabled = () -> false;
-    this.pidModeDisabled = () -> false;
-  }
-
-  @Override
-  public void periodic() {
-    io.updateInputs(inputs);
-    inputs.stalledExtended = stalledExtend;
-    inputs.stalledCollapsed = stalledCollapse;
-    inputs.stallExtendTimer = stallExtendTimer.get();
-    inputs.stallCollapseTimer = stallCollapseTimer.get();
-    Logger.processInputs("Intake", inputs);
-    RobotState.getInstance().intaking = inputs.intakeVolts > 0;
-    if (inputs.setpointValue == COLLAPSE_POS
-        && Math.abs(inputs.getExtendPos - inputs.setpointValue) < 10) {
-      RobotState.getInstance().intakePosition = IntakePosition.STOWED;
-    }
-    if (inputs.setpointValue == EXTEND_POS
-        && Math.abs(inputs.getExtendPos - inputs.setpointValue) < 10) {
-      RobotState.getInstance().intakePosition = IntakePosition.DEPLOYED;
-    }
-
-    // setPID mode in io to true or false depending on the passed in trigger
-    if (pidModeDisabled.getAsBoolean() == io.getPIDEnabled()) {
-      io.setPIDEnabled(!pidModeDisabled.getAsBoolean());
-    }
-    // Non-slipping control calibration based on the limit switch state
-    if (!limitSwitchDisabled.getAsBoolean() && isHopperCollapsed()) {
-      io.resetExtendEncoder(0);
-      isStowed = true;
-      stalledCollapse = false;
-    }
-
-    // Slipping system
-    if (limitSwitchDisabled.getAsBoolean()) {
-
-      // If we are stalling in extend, start timing, otherwise stop timing
-      if (!stalledExtend && isStallingExtend()) {
-        stallExtendTimer.start();
-      } else {
-        stallExtendTimer.stop();
-        stallExtendTimer.reset();
-      }
-
-      // If we are not stalled extended but the timer has exceeded the stall time, set to stalled
-      if (!stalledExtend && stallExtendTimer.get() > STALL_TIME) {
-        stalledExtend = true;
-        stallExtendTimer.stop();
-        stallExtendTimer.reset();
-        io.resetExtendEncoder(EXTEND_POS);
-        isExtended = true;
-      }
-
-      // If we are stalling in collapse, start timing, otherwise stop timing
-      if (!stalledCollapse && isStallingCollapse()) {
-        stallCollapseTimer.start();
-      } else {
-        stallCollapseTimer.stop();
-        stallCollapseTimer.reset();
-      }
-
-      // If we are not stalled collapsed but the timer has exceeded the stall time, set to stalled
-      if (!stalledCollapse && stallCollapseTimer.get() > STALL_TIME) {
-        stalledCollapse = true;
-        stallCollapseTimer.stop();
-        stallCollapseTimer.reset();
-        io.resetExtendEncoder(0);
-        isStowed = true;
-      }
-
-      // If we are moving, we are not stalled
-      if (isMoving()) {
-        stalledExtend = false;
-        stalledCollapse = false;
-        isExtended = false;
-        isStowed = false;
-      }
-    }
-
-    Logger.recordOutput("Intake/StalledExtend", stalledExtend);
-    Logger.recordOutput("Intake/StalledCollapse", stalledCollapse);
-  }
-
-  private void setPercentIntake(double intakePercent) {
-    io.setPercentIntake(intakePercent);
-  }
-
-  private void setPercentExtend(double extendPercent) {
-    io.setPercentExtend(extendPercent);
-  }
-
-  private void setVoltageIntake(double intakeVolts) {
-    io.setVoltageIntake(intakeVolts);
-  }
-
-  private void setVoltageExtend(double extendVolts) {
-    io.setVoltageExtend(extendVolts);
-  }
-
-  private void stopIntake() {
-    io.setVoltageIntake(0);
-  }
-
-  private void stopExtend() {
-    io.setVoltageExtend(0);
-  }
-
-  private boolean isHopperCollapsed() {
-    return io.isCollapsed();
-  }
-
-  private boolean isStallingExtend() {
-    // For our volts, we are not getting the right velocity
-    return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts > EXTEND_VOLTS;
-  }
-
-  private boolean isStallingCollapse() {
-    return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts < COLLAPSE_VOLTS;
-  }
-
-  public Command resetExtendPosition() {
-    return Commands.runOnce(() -> io.resetExtendEncoder(0), this);
-  }
-
-  private boolean isMoving() {
-    return Math.abs(inputs.extendVelocity) > STALL_VELOCITY;
-  }
-
-  public Command intake() {
-    return Commands.run(() -> setVoltageIntake(INTAKE_VOLTS), this)
-        .withName("intake")
-        .finallyDo(this::stopIntake);
-  }
-
-  public Command outtake() {
-    return Commands.run(() -> setVoltageIntake(OUTTAKE_VOLTS), this)
-        .withName("outtake")
-        .finallyDo(this::stopIntake);
-  }
-
-  public Command stopIntakeCommand() {
-    return Commands.run(this::stopIntake, this).withName("stopIntakeCommand");
-  }
-
-  public Command runIntakeExtendVolts(double volts) {
-    return Commands.run(
-        () -> {
-          io.setPIDEnabled(false);
-          io.setVoltageExtend(volts);
-        },
-        this);
-  }
-
-  public Command stopExtendingCommand() {
-    return Commands.run(this::stopExtend, this);
+  public Intake(IntakeRollers rollers, IntakeExtend extend) {
+    this.rollers = rollers;
+    this.extend = extend;
   }
 
   public Command extendToAngleAndIntake(double angle) {
-    return Commands.run(
-            () -> {
-              io.setPIDEnabled(true);
-              io.goToPos(angle);
-              io.setVoltageIntake(INTAKE_VOLTS);
-            },
-            this)
-        .until(() -> inputs.atSetpoint)
-        .finallyDo(this::stopExtend)
+    return Commands.parallel(extend.extendToAngle(angle), rollers.intake())
         .withName("extendToAngleAndIntake");
   }
 
   public Command extendToAngle(double angle) {
-    return Commands.run(
-            () -> {
-              io.setPIDEnabled(true);
-              io.goToPos(angle);
-            },
-            this)
-        .until(() -> inputs.atSetpoint)
-        .finallyDo(this::stopExtend)
-        .withName("extendToAngle");
+    return extend.extendToAngle(angle);
   }
 
   public Command holdAngleAndIntake(double angle) {
-    return Commands.run(
-            () -> {
-              io.setPIDEnabled(true);
-              io.goToPos(angle);
-              io.setVoltageIntake(INTAKE_VOLTS);
-            },
-            this)
-        .finallyDo(
-            () -> {
-              stopExtend();
-              stopIntake();
-            })
+    return Commands.parallel(extend.holdAngle(angle), rollers.intake())
         .withName("holdAngleAndIntake");
   }
 
   // Jack's Chugga Chugga mode
 
-  //  public Command shakeAndIntake() {
-  //    return Commands.repeatingSequence(
-  //            Commands.runOnce(
-  //                () ->
-  //                    Commands.deadline(
-  //                        moveToAnglePrivate(FUNNEL_POS + SHAKE_POS_OFFSET), intakePrivate()),
-  //                this),
-  //            Commands.runOnce(
-  //                () ->
-  //                    Commands.deadline(
-  //                        moveToAnglePrivate(FUNNEL_POS + SHAKE_POS_OFFSET), intakePrivate()),
-  //                this))
-  //        .withName("shakeAndIntake");
-  //  }
+  public Command shakeAndIntake() {
+    return Commands.repeatingSequence(
+            Commands.deadline(
+                extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), rollers.intake()),
+            Commands.deadline(
+                extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), rollers.intake()))
+        .withName("shakeAndIntake");
+  }
 
   // private because it doesn't have requirements and therefore it shouldn't be called beyond the
   // subsystem
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index fe3914124..fc514cf86 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -30,5 +30,6 @@ public class IntakeConstants {
   public static final double COLLAPSE_POS = 4.0;
   public static final double POSITION_TOLERANCE = 7;
 
-  public static final double INTAKE_INTAKE_MOTOR_CURRENT_LIMIT = 45;
+  public static final double INTAKE_EXTEND_MOTOR_CURRENT_LIMIT = 20;
+  public static final double INTAKE_ROLLERS_MOTOR_CURRENT_LIMIT = 45;
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
deleted file mode 100644
index 21cfef84a..000000000
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ /dev/null
@@ -1,54 +0,0 @@
-package frc.robot.subsystems.intake;
-
-import org.littletonrobotics.junction.AutoLog;
-
-public interface IntakeIO {
-  @AutoLog
-  class IntakeIOInputs {
-    public double intakePercentOutput = 0.0;
-    public double extendPercentOutput = 0.0;
-    public double intakeVolts = 0.0;
-    public double extendVolts = 0.0;
-    public double intakeAmps = 0.0;
-    public double extendVelocity = 0.0;
-    public double extendAmps = 0.0;
-    public boolean isCollapsed = false;
-    // public boolean manualModeInput;
-    public boolean atSetpoint = false;
-    public boolean hasSetpoint = false;
-    public double setpointValue = 0.0;
-    public double getExtendPos = 0.0;
-    public boolean stalledExtended = false;
-    public boolean stalledCollapsed = false;
-    public double stallExtendTimer = 0.0;
-    public double stallCollapseTimer = 0.0;
-  }
-
-  default void updateInputs(IntakeIOInputs inputs) {}
-
-  default void setPercentIntake(double intakePercent) {}
-
-  default void setPercentExtend(double extendPercent) {}
-
-  default void setVoltageIntake(double intakeVolts) {}
-
-  default void setVoltageExtend(double extendVolts) {}
-
-  default void stop() {}
-
-  default void goToPos(double pos) {}
-
-  default void setPIDEnabled(boolean enabled) {}
-
-  default boolean getPIDEnabled() {
-    return false;
-  }
-
-  default void resetExtendEncoder(double position) {}
-
-  default boolean isCollapsed() {
-    return false;
-  }
-
-  default void extendToPosition(double position) {}
-}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOKraken.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOKraken.java
deleted file mode 100644
index b9ee3ca05..000000000
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOKraken.java
+++ /dev/null
@@ -1,155 +0,0 @@
-package frc.robot.subsystems.intake;
-
-import static frc.lib.utils.PhoenixUtil.tryUntilOk;
-import static frc.robot.Schematic.intakeExtendCanId;
-import static frc.robot.Schematic.intakeMotorCanId;
-import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_LIMIT_ID;
-import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POSITION_CONVERSION;
-import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_VELOCITY_CONVERSION;
-import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_INTAKE_MOTOR_CURRENT_LIMIT;
-import static frc.robot.subsystems.intake.IntakeConstants.PID_D;
-import static frc.robot.subsystems.intake.IntakeConstants.PID_I;
-import static frc.robot.subsystems.intake.IntakeConstants.PID_P;
-
-import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.EncoderConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.units.measure.Current;
-import edu.wpi.first.units.measure.Voltage;
-import edu.wpi.first.wpilibj.DigitalInput;
-
-public class IntakeIOKraken implements IntakeIO {
-
-  private final TalonFX intakeMotor;
-  private final SparkMax extendMotor;
-  private final DigitalInput collapsedLimitSwitch;
-  private final RelativeEncoder extendMotorEncoder;
-  private final SparkClosedLoopController pidController;
-  private double setPos = 0;
-  private boolean usingPID = false;
-
-  private final StatusSignal<Voltage> intakeAppliedVolts;
-  private final StatusSignal<Current> intakeCurrent;
-
-  public IntakeIOKraken() {
-    intakeMotor = new TalonFX(intakeMotorCanId);
-    extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless);
-
-    var intakeConfig = new TalonFXConfiguration();
-    intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-    intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-    intakeConfig.CurrentLimits.StatorCurrentLimit = INTAKE_INTAKE_MOTOR_CURRENT_LIMIT;
-    intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true;
-
-    var extendConfig = new SparkMaxConfig();
-    extendConfig
-        .inverted(false)
-        .idleMode(IdleMode.kCoast)
-        .voltageCompensation(12)
-        .smartCurrentLimit(30)
-        .closedLoop
-        .pid(PID_P, PID_I, PID_D);
-
-    tryUntilOk(5, () -> intakeMotor.getConfigurator().apply(intakeConfig));
-
-    intakeAppliedVolts = intakeMotor.getMotorVoltage();
-    intakeCurrent = intakeMotor.getStatorCurrent();
-
-    BaseStatusSignal.setUpdateFrequencyForAll(50.0, intakeAppliedVolts, intakeCurrent);
-
-    EncoderConfig extendEnc = new EncoderConfig();
-    extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION);
-    extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION);
-    extendConfig.apply(extendEnc);
-
-    pidController = extendMotor.getClosedLoopController();
-    extendMotorEncoder = extendMotor.getEncoder();
-
-    extendMotor.configure(
-        extendConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-    collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID);
-  }
-
-  @Override
-  public void updateInputs(IntakeIOInputs inputs) {
-    BaseStatusSignal.refreshAll(intakeAppliedVolts, intakeCurrent);
-
-    inputs.extendPercentOutput = extendMotor.get();
-    inputs.extendVelocity = extendMotorEncoder.getVelocity();
-    inputs.intakeVolts = intakeAppliedVolts.getValueAsDouble();
-    inputs.extendVolts = extendMotor.getAppliedOutput();
-    inputs.intakeAmps = intakeCurrent.getValueAsDouble();
-    inputs.extendAmps = extendMotor.getOutputCurrent();
-    inputs.isCollapsed = !collapsedLimitSwitch.get();
-    inputs.getExtendPos = extendMotorEncoder.getPosition();
-    inputs.hasSetpoint = usingPID;
-    inputs.setpointValue = extendMotor.getClosedLoopController().getSetpoint();
-    inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint();
-  }
-
-  @Override
-  public void extendToPosition(double position) {
-    pidController.setSetpoint(position, ControlType.kPosition);
-  }
-
-  @Override
-  public void setPercentIntake(double intakePercent) {
-    intakeMotor.set(intakePercent);
-  }
-
-  @Override
-  public void setPercentExtend(double extendPercent) {
-    extendMotor.set(extendPercent);
-  }
-
-  @Override
-  public void setVoltageIntake(double intakeVolts) {
-    intakeMotor.setVoltage(intakeVolts);
-  }
-
-  @Override
-  public void setVoltageExtend(double extendVolts) {
-    extendMotor.setVoltage(extendVolts);
-  }
-
-  @Override
-  public void goToPos(double setPos) {
-    this.setPos = setPos;
-    if (usingPID && !((isCollapsed() && (setPos > 0)))) {
-      pidController.setSetpoint(setPos, ControlType.kPosition);
-    }
-  }
-
-  @Override
-  public boolean isCollapsed() {
-    return !collapsedLimitSwitch.get();
-  }
-
-  @Override
-  public void setPIDEnabled(boolean enabled) {
-    this.usingPID = enabled;
-  }
-
-  @Override
-  public boolean getPIDEnabled() {
-    return usingPID;
-  }
-
-  @Override
-  public void resetExtendEncoder(double position) {
-    extendMotorEncoder.setPosition(position);
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java
deleted file mode 100644
index e84851154..000000000
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java
+++ /dev/null
@@ -1,136 +0,0 @@
-package frc.robot.subsystems.intake;
-
-import static frc.robot.Schematic.intakeExtendCanId;
-import static frc.robot.Schematic.intakeMotorCanId;
-import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_LIMIT_ID;
-import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POSITION_CONVERSION;
-import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_VELOCITY_CONVERSION;
-import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_POSITION_CONVERSION;
-import static frc.robot.subsystems.intake.IntakeConstants.PID_D;
-import static frc.robot.subsystems.intake.IntakeConstants.PID_I;
-import static frc.robot.subsystems.intake.IntakeConstants.PID_P;
-
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.EncoderConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.wpilibj.DigitalInput;
-
-public class IntakeIOSparkMax implements IntakeIO {
-
-  private final SparkMax intakeMotor;
-  private final SparkMax extendMotor;
-  private final DigitalInput collapsedLimitSwitch;
-  private final RelativeEncoder extendMotorEncoder;
-  private final SparkClosedLoopController pidController;
-  private double setPos = 0;
-  private boolean usingPID = false;
-
-  public IntakeIOSparkMax() {
-    intakeMotor = new SparkMax(intakeMotorCanId, MotorType.kBrushless);
-    extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless);
-
-    var intakeConfig = new SparkMaxConfig();
-    intakeConfig
-        .inverted(false)
-        .idleMode(IdleMode.kCoast)
-        .voltageCompensation(12)
-        .smartCurrentLimit(30);
-    var extendConfig = new SparkMaxConfig();
-    extendConfig
-        .inverted(false)
-        .idleMode(IdleMode.kBrake)
-        .voltageCompensation(12)
-        .smartCurrentLimit(30)
-        .closedLoop
-        .pid(PID_P, PID_I, PID_D);
-
-    EncoderConfig intakeEnc = new EncoderConfig();
-    intakeEnc.positionConversionFactor(INTAKE_POSITION_CONVERSION);
-    intakeEnc.velocityConversionFactor(INTAKE_POSITION_CONVERSION);
-    intakeConfig.apply(intakeEnc);
-
-    EncoderConfig extendEnc = new EncoderConfig();
-    extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION);
-    extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION);
-    extendConfig.apply(extendEnc);
-
-    pidController = extendMotor.getClosedLoopController();
-    extendMotorEncoder = extendMotor.getEncoder();
-
-    intakeMotor.configure(
-        intakeConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-    extendMotor.configure(
-        extendConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-    collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID);
-  }
-
-  @Override
-  public void updateInputs(IntakeIOInputs inputs) {
-    inputs.intakePercentOutput = intakeMotor.get();
-    inputs.extendPercentOutput = extendMotor.get();
-    inputs.extendVelocity = extendMotorEncoder.getVelocity();
-    inputs.intakeVolts = intakeMotor.getAppliedOutput();
-    inputs.extendVolts = extendMotor.getAppliedOutput();
-    inputs.intakeAmps = intakeMotor.getOutputCurrent();
-    inputs.extendAmps = extendMotor.getOutputCurrent();
-    inputs.isCollapsed = collapsedLimitSwitch.get();
-    inputs.getExtendPos = extendMotorEncoder.getPosition();
-    inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint() && usingPID;
-    inputs.hasSetpoint = usingPID;
-    inputs.setpointValue = usingPID ? extendMotor.getClosedLoopController().getSetpoint() : 0.0;
-  }
-
-  @Override
-  public void extendToPosition(double position) {
-    pidController.setSetpoint(position, ControlType.kPosition);
-  }
-
-  @Override
-  public boolean isCollapsed() {
-    return collapsedLimitSwitch.get();
-  }
-
-  @Override
-  public void setPercentIntake(double intakePercent) {
-    intakeMotor.set(intakePercent);
-  }
-
-  @Override
-  public void setPercentExtend(double extendPercent) {
-    extendMotor.set(extendPercent);
-  }
-
-  @Override
-  public void setVoltageIntake(double intakeVolts) {
-    intakeMotor.setVoltage(intakeVolts);
-  }
-
-  @Override
-  public void setVoltageExtend(double extendVolts) {
-    extendMotor.setVoltage(extendVolts);
-  }
-
-  @Override
-  public void goToPos(double setPos) {
-    this.setPos = setPos;
-    if (usingPID) {
-      pidController.setSetpoint(setPos, ControlType.kPosition);
-    }
-  }
-
-  public void setPIDEnabled(boolean enabled) {
-    this.usingPID = enabled;
-  }
-
-  @Override
-  public void resetExtendEncoder(double position) {
-    extendMotorEncoder.setPosition(position);
-  }
-}

From c09e5919383dc79555cdc332fc93b0d35e465d12 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Fri, 10 Apr 2026 21:34:28 -0400
Subject: [PATCH 024/102] update intake

---
 .../intake/extend/IntakeExtend.java           | 140 ++++++++++++++++++
 .../intake/extend/IntakeExtendIO.java         |  49 ++++++
 .../intake/extend/IntakeExtendIOKraken.java   | 121 +++++++++++++++
 .../intake/extend/IntakeExtendIOSparkMax.java | 108 ++++++++++++++
 .../intake/rollers/IntakeRollers.java         |  53 +++++++
 .../intake/rollers/IntakeRollersIO.java       |  22 +++
 .../intake/rollers/IntakeRollersIOKraken.java |  58 ++++++++
 .../rollers/IntakeRollersIOSparkMax.java      |  56 +++++++
 8 files changed, 607 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
 create mode 100644 src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java
 create mode 100644 src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java
 create mode 100644 src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java
 create mode 100644 src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java
 create mode 100644 src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java
 create mode 100644 src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java
 create mode 100644 src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java

diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
new file mode 100644
index 000000000..cb9e8a46f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -0,0 +1,140 @@
+package frc.robot.subsystems.intake.extend;
+
+import static frc.robot.subsystems.intake.IntakeConstants.COLLAPSE_POS;
+import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POS;
+import static frc.robot.subsystems.intake.IntakeConstants.POSITION_TOLERANCE;
+import static frc.robot.subsystems.intake.IntakeConstants.STALL_VELOCITY;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotState;
+import frc.robot.RobotState.IntakePosition;
+import java.util.function.BooleanSupplier;
+import org.littletonrobotics.junction.Logger;
+
+public class IntakeExtend extends SubsystemBase {
+  private final IntakeExtendIO io;
+  private final IntakeExtendIOInputsAutoLogged inputs = new IntakeExtendIOInputsAutoLogged();
+  private final Timer stallExtendTimer = new Timer();
+  private final Timer stallCollapseTimer = new Timer();
+
+  private boolean stalledExtend = false;
+  private boolean stalledCollapse = false;
+  private boolean isStowed = false;
+  private boolean isExtended = false;
+
+  private double targetExtendPosition = 0;
+
+  /**
+   * @param limitSwitchDisabled A supplier to return whether the limit switch is currently disabled
+   *     or not. If disabled, uses slipping to control intake.
+   */
+  private final BooleanSupplier limitSwitchDisabled;
+
+  public void periodic() {
+    inputs.stalledExtended = stalledExtend;
+    inputs.stalledCollapsed = stalledCollapse;
+    inputs.stowed = isStowed;
+    inputs.stallExtendTimer = stallExtendTimer.get();
+    inputs.stallCollapseTimer = stallCollapseTimer.get();
+    io.updateInputs(inputs);
+    Logger.processInputs("Intake", inputs);
+    Logger.recordOutput("Intake/IntakeExtend/StalledExtend", stalledExtend);
+    Logger.recordOutput("Intake/IntakeExtended/StalledCollapse", stalledCollapse);
+    Logger.recordOutput("Intake/stallingExtend", isStallingExtend());
+    Logger.recordOutput("Intake/stallingCollapse", isStallingCollapse());
+    if (inputs.getExtendPos > EXTEND_POS / 2) {
+      RobotState.getInstance().intakePosition = IntakePosition.STOWED;
+    }
+    if (inputs.getExtendPos <= EXTEND_POS / 2) {
+      RobotState.getInstance().intakePosition = IntakePosition.DEPLOYED;
+    }
+    if (inputs.isCollapsed) {
+      io.resetExtendEncoder(0.0);
+    }
+  }
+
+  public IntakeExtend(IntakeExtendIO io, BooleanSupplier limitSwitchDisabled) {
+    this.io = io;
+    this.limitSwitchDisabled = limitSwitchDisabled;
+  }
+
+  public boolean isHopperCollapsed() {
+    return io.isCollapsed();
+  }
+
+  private boolean isStallingExtend() {
+    // For our volts, we are not getting the right velocity
+    return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts < -0.01;
+  }
+
+  private boolean isStallingCollapse() {
+    return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts > 0.01;
+  }
+
+  private void setPercentExtend(double extendPercent) {
+    io.setPercentExtend(extendPercent);
+  }
+
+  public void setVoltageExtend(double extendVolts) {
+    io.setVoltageExtend(extendVolts);
+  }
+
+  public void stopExtend() {
+    io.setVoltageExtend(0);
+  }
+
+  public Command resetExtendPosition() {
+    return Commands.runOnce(() -> io.resetExtendEncoder(0), this);
+  }
+
+  public Command moveToExtendedPosition() {
+    return Commands.run(() -> io.extendToPosition(EXTEND_POS))
+        .until(() -> Math.abs(inputs.getExtendPos - EXTEND_POS) <= POSITION_TOLERANCE)
+        .withName("moveToExtendedPosition");
+  }
+
+  public Command moveToCollapsedPosition() {
+    return Commands.run(() -> io.extendToPosition(COLLAPSE_POS))
+        .until(() -> Math.abs(inputs.getExtendPos - COLLAPSE_POS) <= POSITION_TOLERANCE)
+        .withName("moveToCollapsedPosition");
+  }
+
+  public Command runIntakeExtendVolts(double volts) {
+    return Commands.run(
+        () -> {
+          io.setPIDEnabled(false);
+          io.setVoltageExtend(volts);
+        },
+        this);
+  }
+
+  public Command stopExtendingCommand() {
+    return Commands.run(this::stopExtend, this);
+  }
+
+  public Command extendToAngle(double angle) {
+    return Commands.run(
+            () -> {
+              io.setPIDEnabled(true);
+              io.goToPos(angle);
+            },
+            this)
+        .until(() -> inputs.atSetpoint)
+        .finallyDo(this::stopExtend)
+        .withName("extendToAngle");
+  }
+
+  public Command holdAngle(double angle) {
+    return Commands.run(
+            () -> {
+              io.setPIDEnabled(true);
+              io.goToPos(angle);
+            },
+            this)
+        .finallyDo(this::stopExtend)
+        .withName("holdAngle");
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java
new file mode 100644
index 000000000..082317d7c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java
@@ -0,0 +1,49 @@
+package frc.robot.subsystems.intake.extend;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeExtendIO {
+
+  @AutoLog
+  class IntakeExtendIOInputs {
+    public double extendPercentOutput = 0.0;
+    public double extendVolts = 0.0;
+    public double extendVelocity = 0.0;
+    public double extendAmps = 0.0;
+    public boolean isCollapsed = false;
+    // public boolean manualModeInput;
+    public boolean atSetpoint = false;
+    public boolean hasSetpoint = false;
+    public double setpointValue = 0.0;
+    public double getExtendPos = 0.0;
+    public boolean stalledExtended = false;
+    public boolean stalledCollapsed = false;
+    public double stallExtendTimer = 0.0;
+    public double stallCollapseTimer = 0.0;
+    public boolean stowed = false;
+  }
+
+  default void updateInputs(IntakeExtendIOInputs inputs) {}
+
+  default void setPercentExtend(double extendPercent) {}
+
+  default void setVoltageExtend(double extendVolts) {}
+
+  default void stop() {}
+
+  default void goToPos(double pos) {}
+
+  default void setPIDEnabled(boolean enabled) {}
+
+  default boolean getPIDEnabled() {
+    return false;
+  }
+
+  default void resetExtendEncoder(double position) {}
+
+  default boolean isCollapsed() {
+    return false;
+  }
+
+  default void extendToPosition(double position) {}
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java
new file mode 100644
index 000000000..814cfc72f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java
@@ -0,0 +1,121 @@
+package frc.robot.subsystems.intake.extend;
+
+import static frc.robot.Schematic.intakeExtendCanId;
+import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_LIMIT_ID;
+import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POSITION_CONVERSION;
+import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_VELOCITY_CONVERSION;
+import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_EXTEND_MOTOR_CURRENT_LIMIT;
+import static frc.robot.subsystems.intake.IntakeConstants.PID_D;
+import static frc.robot.subsystems.intake.IntakeConstants.PID_I;
+import static frc.robot.subsystems.intake.IntakeConstants.PID_P;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public class IntakeExtendIOKraken implements IntakeExtendIO {
+  private final SparkMax extendMotor;
+  private final DigitalInput collapsedLimitSwitch;
+  private final RelativeEncoder extendMotorEncoder;
+  private final SparkClosedLoopController pidController;
+  private double setPos = 0;
+  private boolean usingPID = false;
+
+  public IntakeExtendIOKraken() {
+    extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless);
+
+    var intakeConfig = new TalonFXConfiguration();
+    intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+    intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    intakeConfig.CurrentLimits.StatorCurrentLimit = INTAKE_EXTEND_MOTOR_CURRENT_LIMIT;
+    intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+
+    var extendConfig = new SparkMaxConfig();
+    extendConfig
+        .inverted(false)
+        .idleMode(IdleMode.kCoast)
+        .voltageCompensation(12)
+        .smartCurrentLimit(30)
+        .closedLoop
+        .pid(PID_P, PID_I, PID_D);
+
+    EncoderConfig extendEnc = new EncoderConfig();
+    extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION);
+    extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION);
+    extendConfig.apply(extendEnc);
+
+    pidController = extendMotor.getClosedLoopController();
+    extendMotorEncoder = extendMotor.getEncoder();
+
+    extendMotor.configure(
+        extendConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID);
+  }
+
+  @Override
+  public void updateInputs(IntakeExtendIOInputs inputs) {
+    inputs.extendPercentOutput = extendMotor.get();
+    inputs.extendVelocity = extendMotorEncoder.getVelocity();
+    inputs.extendVolts = extendMotor.getAppliedOutput();
+    inputs.extendAmps = extendMotor.getOutputCurrent();
+    inputs.isCollapsed = !collapsedLimitSwitch.get();
+    inputs.getExtendPos = extendMotorEncoder.getPosition();
+    inputs.hasSetpoint = usingPID;
+    inputs.setpointValue = extendMotor.getClosedLoopController().getSetpoint();
+    inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint();
+  }
+
+  @Override
+  public void extendToPosition(double position) {
+    pidController.setSetpoint(position, ControlType.kPosition);
+  }
+
+  @Override
+  public void setPercentExtend(double extendPercent) {
+    extendMotor.set(extendPercent);
+  }
+
+  @Override
+  public void setVoltageExtend(double extendVolts) {
+    extendMotor.setVoltage(extendVolts);
+  }
+
+  @Override
+  public void goToPos(double setPos) {
+    this.setPos = setPos;
+    if (usingPID && !((isCollapsed() && (setPos > 0)))) {
+      pidController.setSetpoint(setPos, ControlType.kPosition);
+    }
+  }
+
+  @Override
+  public boolean isCollapsed() {
+    return !collapsedLimitSwitch.get();
+  }
+
+  @Override
+  public void setPIDEnabled(boolean enabled) {
+    this.usingPID = enabled;
+  }
+
+  @Override
+  public boolean getPIDEnabled() {
+    return usingPID;
+  }
+
+  @Override
+  public void resetExtendEncoder(double position) {
+    extendMotorEncoder.setPosition(position);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java
new file mode 100644
index 000000000..912229481
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java
@@ -0,0 +1,108 @@
+package frc.robot.subsystems.intake.extend;
+
+import static frc.robot.Schematic.intakeExtendCanId;
+import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_LIMIT_ID;
+import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POSITION_CONVERSION;
+import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_VELOCITY_CONVERSION;
+import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_EXTEND_MOTOR_CURRENT_LIMIT;
+import static frc.robot.subsystems.intake.IntakeConstants.PID_D;
+import static frc.robot.subsystems.intake.IntakeConstants.PID_I;
+import static frc.robot.subsystems.intake.IntakeConstants.PID_P;
+
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.AlternateEncoderConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public class IntakeExtendIOSparkMax implements IntakeExtendIO {
+
+  private final SparkMax extendMotor;
+  private final DigitalInput collapsedLimitSwitch;
+  private final RelativeEncoder extendMotorEncoder;
+  private final SparkClosedLoopController pidController;
+  private double setPos = 0;
+  private boolean usingPID = false;
+
+  public IntakeExtendIOSparkMax() {
+    extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless);
+    AlternateEncoderConfig extendEnc = new AlternateEncoderConfig();
+    extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION);
+    extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION);
+    extendEnc.inverted(true);
+    var extendConfig = new SparkMaxConfig();
+    extendConfig
+        .inverted(false)
+        .idleMode(IdleMode.kCoast)
+        .voltageCompensation(12)
+        .smartCurrentLimit((int) Math.round(INTAKE_EXTEND_MOTOR_CURRENT_LIMIT))
+        .closedLoop
+        .pid(PID_P, PID_I, PID_D)
+        .feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder);
+    extendConfig.alternateEncoder.apply(extendEnc);
+
+    pidController = extendMotor.getClosedLoopController();
+    extendMotorEncoder = extendMotor.getAlternateEncoder();
+
+    extendMotor.configure(
+        extendConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID);
+  }
+
+  @Override
+  public void updateInputs(IntakeExtendIOInputs inputs) {
+    inputs.extendPercentOutput = extendMotor.get();
+    inputs.extendVelocity = extendMotorEncoder.getVelocity();
+    inputs.extendVolts = extendMotor.getAppliedOutput();
+    inputs.extendAmps = extendMotor.getOutputCurrent();
+    inputs.isCollapsed = collapsedLimitSwitch.get();
+    inputs.getExtendPos = extendMotorEncoder.getPosition();
+    inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint() && usingPID;
+    inputs.hasSetpoint = usingPID;
+    inputs.setpointValue = extendMotor.getClosedLoopController().getSetpoint();
+  }
+
+  @Override
+  public void extendToPosition(double position) {
+    pidController.setSetpoint(position, ControlType.kPosition);
+  }
+
+  @Override
+  public boolean isCollapsed() {
+    return collapsedLimitSwitch.get();
+  }
+
+  @Override
+  public void setPercentExtend(double extendPercent) {
+    extendMotor.set(extendPercent);
+  }
+
+  @Override
+  public void setVoltageExtend(double extendVolts) {
+    extendMotor.setVoltage(extendVolts);
+  }
+
+  @Override
+  public void goToPos(double setPos) {
+    this.setPos = setPos;
+    if (usingPID) {
+      pidController.setSetpoint(setPos, ControlType.kPosition);
+    }
+  }
+
+  public void setPIDEnabled(boolean enabled) {
+    this.usingPID = enabled;
+  }
+
+  @Override
+  public void resetExtendEncoder(double position) {
+    extendMotorEncoder.setPosition(position);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java
new file mode 100644
index 000000000..0cec68ba6
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java
@@ -0,0 +1,53 @@
+package frc.robot.subsystems.intake.rollers;
+
+import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_VOLTS;
+import static frc.robot.subsystems.intake.IntakeConstants.OUTTAKE_VOLTS;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotState;
+import org.littletonrobotics.junction.Logger;
+
+public class IntakeRollers extends SubsystemBase {
+  private final IntakeRollersIO io;
+  private final IntakeRollersIOInputsAutoLogged inputs = new IntakeRollersIOInputsAutoLogged();
+
+  public IntakeRollers(IntakeRollersIO io) {
+    this.io = io;
+  }
+
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Intake/IntakeRollers", inputs);
+    RobotState.getInstance().intaking = inputs.intakeVolts > 0;
+  }
+
+  private void setPercentIntake(double intakePercent) {
+    io.setPercentIntake(intakePercent);
+  }
+
+  public void setVoltageIntake(double intakeVolts) {
+    io.setVoltageIntake(intakeVolts);
+  }
+
+  private void stopIntake() {
+    io.setVoltageIntake(0);
+  }
+
+  public Command intake() {
+    return Commands.run(() -> setVoltageIntake(INTAKE_VOLTS), this)
+        .finallyDo(this::stopIntake)
+        .withName("intake");
+  }
+
+  public Command outtake() {
+    return Commands.run(() -> setVoltageIntake(OUTTAKE_VOLTS), this)
+        .finallyDo(this::stopIntake)
+        .withName("outtake");
+  }
+
+  public Command stopIntakeCommand() {
+    return Commands.run(this::stopIntake).withName("stopIntakeCommand");
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java
new file mode 100644
index 000000000..305898aa5
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java
@@ -0,0 +1,22 @@
+package frc.robot.subsystems.intake.rollers;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeRollersIO {
+  @AutoLog
+  class IntakeRollersIOInputs {
+    public double intakePercentOutput = 0.0;
+    public double intakeVolts = 0.0;
+    public double intakeAmps = 0.0;
+    public double intakeRPM = 0.0;
+    // public boolean manualModeInput;
+  }
+
+  default void updateInputs(IntakeRollersIOInputs inputs) {}
+
+  default void setPercentIntake(double intakePercent) {}
+
+  default void setVoltageIntake(double intakeVolts) {}
+
+  default void stop() {}
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java
new file mode 100644
index 000000000..cbe116e01
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java
@@ -0,0 +1,58 @@
+package frc.robot.subsystems.intake.rollers;
+
+import static frc.lib.utils.PhoenixUtil.tryUntilOk;
+import static frc.robot.Schematic.intakeMotorCanId;
+import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_ROLLERS_MOTOR_CURRENT_LIMIT;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+
+public class IntakeRollersIOKraken implements IntakeRollersIO {
+
+  private final TalonFX intakeMotor;
+  private double setPos = 0;
+
+  private final StatusSignal<Voltage> intakeAppliedVolts;
+  private final StatusSignal<Current> intakeCurrent;
+
+  public IntakeRollersIOKraken() {
+    intakeMotor = new TalonFX(intakeMotorCanId);
+
+    var intakeConfig = new TalonFXConfiguration();
+    intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    intakeConfig.CurrentLimits.StatorCurrentLimit = INTAKE_ROLLERS_MOTOR_CURRENT_LIMIT;
+    intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+
+    tryUntilOk(5, () -> intakeMotor.getConfigurator().apply(intakeConfig));
+
+    intakeAppliedVolts = intakeMotor.getMotorVoltage();
+    intakeCurrent = intakeMotor.getStatorCurrent();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(50.0, intakeAppliedVolts, intakeCurrent);
+  }
+
+  @Override
+  public void updateInputs(IntakeRollersIOInputs inputs) {
+    BaseStatusSignal.refreshAll(intakeAppliedVolts, intakeCurrent);
+    inputs.intakeVolts = intakeAppliedVolts.getValueAsDouble();
+    inputs.intakeAmps = intakeCurrent.getValueAsDouble();
+    inputs.intakeRPM = intakeMotor.getVelocity().getValueAsDouble() * 60;
+  }
+
+  @Override
+  public void setPercentIntake(double intakePercent) {
+    intakeMotor.set(intakePercent);
+  }
+
+  @Override
+  public void setVoltageIntake(double intakeVolts) {
+    intakeMotor.setVoltage(intakeVolts);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java
new file mode 100644
index 000000000..39b4ded2e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java
@@ -0,0 +1,56 @@
+package frc.robot.subsystems.intake.rollers;
+
+import static frc.robot.Schematic.intakeMotorCanId;
+import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_EXTEND_MOTOR_CURRENT_LIMIT;
+import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_POSITION_CONVERSION;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.EncoderConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class IntakeRollersIOSparkMax implements IntakeRollersIO {
+
+  private final SparkMax intakeMotor;
+  private double setPos = 0;
+  private boolean usingPID = false;
+
+  public IntakeRollersIOSparkMax() {
+    intakeMotor = new SparkMax(intakeMotorCanId, MotorType.kBrushless);
+
+    var intakeConfig = new SparkMaxConfig();
+    intakeConfig
+        .inverted(false)
+        .idleMode(IdleMode.kCoast)
+        .voltageCompensation(12)
+        .smartCurrentLimit((int) Math.round(INTAKE_EXTEND_MOTOR_CURRENT_LIMIT));
+
+    EncoderConfig intakeEnc = new EncoderConfig();
+    intakeEnc.positionConversionFactor(INTAKE_POSITION_CONVERSION);
+    intakeEnc.velocityConversionFactor(INTAKE_POSITION_CONVERSION);
+    intakeConfig.apply(intakeEnc);
+  }
+
+  @Override
+  public void updateInputs(IntakeRollersIOInputs inputs) {
+    inputs.intakePercentOutput = intakeMotor.get();
+    inputs.intakeVolts = intakeMotor.getAppliedOutput();
+    inputs.intakeAmps = intakeMotor.getOutputCurrent();
+    inputs.intakeRPM = intakeMotor.getEncoder().getVelocity();
+  }
+
+  @Override
+  public void setPercentIntake(double intakePercent) {
+    intakeMotor.set(intakePercent);
+  }
+
+  @Override
+  public void setVoltageIntake(double intakeVolts) {
+    intakeMotor.setVoltage(intakeVolts);
+  }
+
+  public void setPIDEnabled(boolean enabled) {
+    this.usingPID = enabled;
+  }
+}

From 454bed34112793df843a1238b2c2acdb945c9522 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Fri, 10 Apr 2026 20:40:07 -0500
Subject: [PATCH 025/102] merge wpi autos into opus codebase

---
 .../java/frc/robot/commands/auto/Autos.java   | 244 ++++++++++++++----
 1 file changed, 191 insertions(+), 53 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 5e8a1925b..112829b83 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -10,6 +10,7 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.rollers.IntakeRollers;
 import frc.robot.subsystems.shooter.Shooter;
 import java.util.function.Supplier;
 
@@ -17,12 +18,19 @@ public class Autos {
   private final Drive drive;
   private final Orchestrator orchestrator;
   private final Intake intake;
+  private final IntakeRollers rollers;
   private final Shooter shooter;
 
-  public Autos(Drive drive, Orchestrator orchestrator, Intake intake, Shooter shooter) {
+  public Autos(
+      Drive drive,
+      Orchestrator orchestrator,
+      Intake intake,
+      IntakeRollers rollers,
+      Shooter shooter) {
     this.drive = drive;
     this.orchestrator = orchestrator;
     this.intake = intake;
+    this.rollers = rollers;
     this.shooter = shooter;
   }
 
@@ -31,23 +39,34 @@ public Autos(Drive drive, Orchestrator orchestrator, Intake intake, Shooter shoo
   private static final Supplier<Pose2d> center = () -> new Pose2d(3.504, 4.019, new Rotation2d(0));
   private static final Supplier<Pose2d> CenterA =
       () -> new Pose2d(3.457, 4.941, new Rotation2d(Units.degreesToRadians(-55.305)));
-  private static final Supplier<Pose2d> firstPoseA =
+  private static final Supplier<Pose2d> intakeSimplePoseA =
       () -> new Pose2d(7.7052903175354, 5.8276801109313965, Rotation2d.fromDegrees(0.0));
-  private static final Supplier<Pose2d> secondPoseA =
+  private static final Supplier<Pose2d> overBumpNeutralPoseA =
+      () -> new Pose2d(6.1, 5.574310302734375, Rotation2d.fromDegrees(0));
+
+  private static final Supplier<Pose2d> intakeComplexFirstPoseA =
+      () -> new Pose2d(6.862, 6.877, Rotation2d.fromDegrees(-90.0));
+  // 6.862
+  private static final Supplier<Pose2d> intakeComplexSecondPoseA =
+      () -> new Pose2d(7.825, 6.877, Rotation2d.fromDegrees(-90));
+
+  private static final Supplier<Pose2d> intakeComplexThirdPoseA =
+      () -> new Pose2d(7.805, 4.461, Rotation2d.fromDegrees(-90));
+  private static final Supplier<Pose2d> overBumpAlliancePoseA =
       () -> new Pose2d(3.0666706562042236, 5.574310302734375, Rotation2d.fromDegrees(0.0));
-  private static final Supplier<Pose2d> secondPoseAAlt1 =
+  private static final Supplier<Pose2d> overBumpAllianceAltPoseA =
       () -> new Pose2d(3.086160182952881, 5.437880039215088, Rotation2d.fromDegrees(0));
-  private static final Supplier<Pose2d> secondPoseAAlt2 =
+  private static final Supplier<Pose2d> shootFromCornerPoseA =
       () ->
           new Pose2d(
               3.086160182952881, 5.437880039215088, Rotation2d.fromRadians(-0.7553977556351216));
-  private static final Supplier<Pose2d> firstPoseB =
+  private static final Supplier<Pose2d> intakeSimplePoseB =
       () -> new Pose2d(7.724780082702637, 2.436420202255249, Rotation2d.fromDegrees(0.0));
-  private static final Supplier<Pose2d> secondPoseB =
+  private static final Supplier<Pose2d> overBumpAlliancePoseB =
       () -> new Pose2d(3.1251401901245117, 2.397440195083618, Rotation2d.fromDegrees(0.0));
-  private static final Supplier<Pose2d> secondPoseBAlt1 =
+  private static final Supplier<Pose2d> overBumpAllianceAltPoseB =
       () -> new Pose2d(3.1251401901245117, 2.397440195083618, Rotation2d.fromDegrees(0));
-  private static final Supplier<Pose2d> secondPoseBAlt2 =
+  private static final Supplier<Pose2d> shootFromCornerPoseB =
       () ->
           new Pose2d(
               3.1251401901245117, 2.397440195083618, Rotation2d.fromRadians(0.7358299996216245));
@@ -56,6 +75,13 @@ public Autos(Drive drive, Orchestrator orchestrator, Intake intake, Shooter shoo
   private static final Supplier<Pose2d> depotPose =
       () -> new Pose2d(0.45501017570495605, 5.983600616455078, Rotation2d.fromDegrees(180));
 
+  // pp
+
+  private static final Supplier<Pose2d> startAside =
+      () -> AllianceFlipUtil.apply(new Pose2d(3.645, 5.520, new Rotation2d(0.000)));
+  private static final Supplier<Pose2d> startBside =
+      () -> AllianceFlipUtil.apply(new Pose2d(3.645, 2.516, new Rotation2d(0.000)));
+
   public Command CenterA() {
     return Commands.sequence(
         Commands.runOnce(() -> drive.setPose(AllianceFlipUtil.apply(CenterA.get()))),
@@ -91,6 +117,78 @@ public Command Bummmmpar() {
             () -> AllianceFlipUtil.apply(new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(0)))));
   }
 
+  public Command ppACycleLeft() {
+    return Commands.sequence(
+        Commands.runOnce(() -> drive.setPose(startAside.get())),
+        Commands.deadline(
+                drive.getAutonomousCommand("A-Cycle-LeftSweep"),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
+                orchestrator.spinUpShooterHub())
+            .withTimeout(14.5),
+        orchestrator.aimToHub().withTimeout(2.5),
+        Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5),
+        Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooter(1214),
+            orchestrator.feedUp()));
+  }
+
+  public Command ppBCycleRight() {
+    return Commands.sequence(
+        Commands.runOnce(() -> drive.setPose(startBside.get())),
+        Commands.deadline(
+                drive.getAutonomousCommand("B-Cycle-RightSweep"),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
+                orchestrator.spinUpShooterHub())
+            .withTimeout(14.5),
+        orchestrator.aimToHub().withTimeout(2.5),
+        Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5),
+        Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooter(1214),
+            orchestrator.feedUp()));
+  }
+
+  public Command ppBCycleRightRegression() {
+    return Commands.sequence(
+        Commands.runOnce(() -> drive.setPose(startBside.get())),
+        Commands.deadline(
+                drive.getAutonomousCommand("B-Cycle-RightSweep"),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+            .withTimeout(14.5),
+        Commands.deadline(
+            orchestrator.aimToHub().withTimeout(2.5),
+            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
+        Commands.parallel(
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                orchestrator.feedUp())
+            .withTimeout(2.5),
+        Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+            orchestrator.feedUp()));
+  }
+
+  public Command ppACycleLeftRegression() {
+    return Commands.sequence(
+        Commands.runOnce(() -> drive.setPose(startAside.get())),
+        Commands.deadline(
+                drive.getAutonomousCommand("A-Cycle-LeftSweep"),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+            .withTimeout(14.5),
+        Commands.deadline(
+            orchestrator.aimToHub().withTimeout(2.5),
+            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
+        Commands.parallel(
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                orchestrator.feedUp())
+            .withTimeout(2.5),
+        Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+            orchestrator.feedUp()));
+  }
+
   public Command EightBalls() {
     return Commands.sequence(
         Commands.deadline(
@@ -101,10 +199,12 @@ public Command EightBalls() {
   public Command ACCManuelAuto() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseA.get())).withTimeout(5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+                .withTimeout(5),
+            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAlliancePoseA.get()))
+            .withTimeout(5),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
         Commands.parallel(
@@ -113,16 +213,42 @@ public Command ACCManuelAuto() {
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))));
   }
 
+  public Command ACCManuelImprovedComplexIntake() {
+    return Commands.sequence(
+        Commands.deadline(
+            Commands.sequence(
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpNeutralPoseA.get()))
+                    .withTimeout(2),
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexFirstPoseA.get())),
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexSecondPoseA.get()))
+                    .withTimeout(1.9),
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexThirdPoseA.get()))
+                    .withTimeout(3),
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpNeutralPoseA.get()))),
+            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
+            .withTimeout(3.5),
+        rollers.stopIntakeCommand().withTimeout(0.05),
+        Commands.deadline(
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
+                .withTimeout(4),
+            orchestrator.spinUpShooter(1240)),
+        Commands.parallel(
+            orchestrator.spinUpShooter(1240),
+            orchestrator.feedUp(),
+            Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))));
+  }
+
   public Command ADepot() {
     return Commands.sequence(
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPoseStopPoint.get()))
             .withTimeout(2),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPose.get())).withTimeout(3.5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        intake.stopIntakeCommand().withTimeout(0.05),
+            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
                 .withTimeout(2),
             orchestrator.spinUpShooter(1250)),
         Commands.deadline(
@@ -130,24 +256,27 @@ public Command ADepot() {
             orchestrator.spinUpShooter(1250),
             orchestrator.feedUp(),
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())).withTimeout(2),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
+            .withTimeout(2),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(3.5),
-            (intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))));
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+                .withTimeout(3.5),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 
   public Command ACCManuelAutoAlt() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+                .withTimeout(5),
+            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
             .withTimeout(3.5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
                 .withTimeout(2),
             orchestrator.spinUpShooter(1240)),
         Commands.parallel(
@@ -159,13 +288,14 @@ public Command ACCManuelAutoAlt() {
   public Command ACCManuelAutoOverBump() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+                .withTimeout(5),
+                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
             .withTimeout(3.5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
                 .withTimeout(2),
             orchestrator.spinUpShooter(1250)),
         Commands.deadline(
@@ -173,21 +303,25 @@ public Command ACCManuelAutoOverBump() {
             orchestrator.spinUpShooter(1250),
             orchestrator.feedUp(),
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())).withTimeout(2),
-        Commands.deadline(new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())))
-            .withTimeout(3.5),
-        Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
+            .withTimeout(2),
+        Commands.deadline(
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+                .withTimeout(3.5),
+                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 
   public Command BCCManuelAuto() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseB.get())).withTimeout(5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
+                .withTimeout(5),
+                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
+            .withTimeout(5),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
         Commands.parallel(
@@ -199,13 +333,14 @@ public Command BCCManuelAuto() {
   public Command BCCManuelAutoAlt() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
+                .withTimeout(5),
+                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
             .withTimeout(3.5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt2.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseB.get()))
                 .withTimeout(2),
             orchestrator.spinUpShooter(1240)),
         Commands.parallel(
@@ -217,13 +352,14 @@ public Command BCCManuelAutoAlt() {
   public Command BCCManuelAutoOverBump() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
+                .withTimeout(5),
+                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
             .withTimeout(3.5),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt2.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseB.get()))
                 .withTimeout(2),
             orchestrator.spinUpShooter(1250)),
         Commands.deadline(
@@ -231,11 +367,13 @@ public Command BCCManuelAutoOverBump() {
             orchestrator.spinUpShooter(1250),
             orchestrator.feedUp(),
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
-        intake.stopIntakeCommand().withTimeout(0.05),
+        rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get())).withTimeout(2),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
+            .withTimeout(2),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(3.5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))));
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
+                .withTimeout(3.5),
+                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 }

From 38b704bbdc5b5b5516cd9318f614c44c6c3e324b Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Fri, 10 Apr 2026 21:49:25 -0400
Subject: [PATCH 026/102] shooter offset

---
 .../java/frc/robot/subsystems/shooter/ShooterConstants.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 03313f6eb..7db7ddb98 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -25,5 +25,5 @@ public class ShooterConstants {
   public static final double TOLERANCE = 50; // measured in radians
 
   public static Transform2d kShooterOffsetFromRobotCenter =
-      new Transform2d(new Translation2d(-0.163, 0.0), new Rotation2d(0.0));
+      new Transform2d(new Translation2d(-0.1839, 0.0), new Rotation2d(0.0));
 }

From 6bfd41f24cf7878f37205e081dd50605711eafc8 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Fri, 10 Apr 2026 20:57:07 -0500
Subject: [PATCH 027/102] 2 cycle autos

---
 .../java/frc/robot/commands/auto/Autos.java   | 96 +++++++++++++++++--
 1 file changed, 86 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index eb251e818..ba8319922 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -11,7 +11,6 @@
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.intake.IntakeConstants;
 import frc.robot.subsystems.intake.rollers.IntakeRollers;
-import frc.robot.subsystems.intake.rollers.IntakeRollers;
 import frc.robot.subsystems.shooter.Shooter;
 import java.util.function.Supplier;
 
@@ -20,7 +19,6 @@ public class Autos {
   private final Orchestrator orchestrator;
   private final Intake intake;
   private final IntakeRollers rollers;
-  private final IntakeRollers rollers;
   private final Shooter shooter;
 
   public Autos(
@@ -33,7 +31,6 @@ public Autos(
     this.orchestrator = orchestrator;
     this.intake = intake;
     this.rollers = rollers;
-    this.rollers = rollers;
     this.shooter = shooter;
   }
 
@@ -152,6 +149,81 @@ public Command ppBCycleRight() {
             orchestrator.feedUp()));
   }
 
+  public Command ppB2CycleRightRegression() {
+    return Commands.sequence(
+        Commands.runOnce(() -> drive.setPose(startBside.get())),
+        Commands.deadline(
+                drive.getAutonomousCommand("B-Cycle1"),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+            .withTimeout(14.5),
+        Commands.deadline(
+            orchestrator.aimToHub().withTimeout(2.5),
+            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
+        Commands.parallel(
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                orchestrator.feedUp())
+            .withTimeout(2.5),
+        Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+            orchestrator.feedUp()))
+        .withTimeout(2.5)
+        .andThen(Commands.sequence(
+            Commands.runOnce(() -> drive.setPose(startBside.get())),
+            Commands.deadline(
+                    drive.getAutonomousCommand("B-Cycle2"),
+                    intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+                .withTimeout(14.5),
+            Commands.deadline(
+                orchestrator.aimToHub().withTimeout(2.5),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
+            Commands.parallel(
+                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                    orchestrator.feedUp())
+                .withTimeout(2.5),
+            Commands.parallel(
+                intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                orchestrator.feedUp())));
+  }
+  public Command ppA2CycleRightRegression() {
+    return Commands.sequence(
+            Commands.runOnce(() -> drive.setPose(startBside.get())),
+            Commands.deadline(
+                    drive.getAutonomousCommand("A-Cycle1"),
+                    intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+                .withTimeout(14.5),
+            Commands.deadline(
+                orchestrator.aimToHub().withTimeout(2.5),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
+            Commands.parallel(
+                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                    orchestrator.feedUp())
+                .withTimeout(2.5),
+            Commands.parallel(
+                intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                orchestrator.feedUp()))
+        .withTimeout(2.5)
+        .andThen(Commands.sequence(
+            Commands.runOnce(() -> drive.setPose(startBside.get())),
+            Commands.deadline(
+                    drive.getAutonomousCommand("A-Cycle2"),
+                    intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+                .withTimeout(14.5),
+            Commands.deadline(
+                orchestrator.aimToHub().withTimeout(2.5),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
+            Commands.parallel(
+                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                    orchestrator.feedUp())
+                .withTimeout(2.5),
+            Commands.parallel(
+                intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                orchestrator.feedUp())));
+  }
+
   public Command ppBCycleRightRegression() {
     return Commands.sequence(
         Commands.runOnce(() -> drive.setPose(startBside.get())),
@@ -202,9 +274,11 @@ public Command EightBalls() {
   public Command ACCManuelAuto() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseA.get())).withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+                .withTimeout(5),
+            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAlliancePoseA.get()))
+            .withTimeout(5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
@@ -246,7 +320,7 @@ public Command ADepot() {
             .withTimeout(2),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPose.get())).withTimeout(3.5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
+            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
@@ -317,9 +391,11 @@ public Command ACCManuelAutoOverBump() {
   public Command BCCManuelAuto() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5),
-            Commands.parallel(intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseB.get())).withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
+                .withTimeout(5),
+                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
+            .withTimeout(5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),

From f39f4719c69f483820fdb5b934b9e99f67ae2338 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Fri, 10 Apr 2026 22:01:46 -0400
Subject: [PATCH 028/102] new numbers

---
 src/main/java/frc/robot/subsystems/drive/DriveConstants.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index 82f8d896e..83593e168 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -15,8 +15,8 @@
 public class DriveConstants {
   public static final double maxSpeedMetersPerSec = 11.8;
   public static final double odometryFrequency = 100.0; // Hz
-  public static final double trackWidth = 0.273 * 2;
-  public static final double wheelBase = 0.488;
+  public static final double trackWidth = 0.59055;
+  public static final double wheelBase = 0.5207;
   public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0);
   public static final Translation2d[] moduleTranslations =
       new Translation2d[] {

From 1cbfc5fd28cb48beee5851be84511ac48797eb70 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Fri, 10 Apr 2026 22:05:20 -0400
Subject: [PATCH 029/102] update name

---
 .../java/frc/robot/subsystems/indexer/IndexConstants.java  | 7 -------
 src/main/java/frc/robot/subsystems/indexer/Indexer.java    | 2 +-
 .../frc/robot/subsystems/indexer/IndexerIOSparkMax.java    | 4 ++--
 3 files changed, 3 insertions(+), 10 deletions(-)
 delete mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexConstants.java

diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexConstants.java
deleted file mode 100644
index 63c232a0a..000000000
--- a/src/main/java/frc/robot/subsystems/indexer/IndexConstants.java
+++ /dev/null
@@ -1,7 +0,0 @@
-package frc.robot.subsystems.indexer;
-
-public class IndexConstants {
-  public static final double ENCODER_FEEDUP_POSITION_CONVERSION = 1;
-  public static final double ENCODER_FEEDUP_VELOCITY_CONVERSION = 1;
-  public static final double FEEDUP_VOLT = 12;
-}
diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
index 215b006df..2c3840830 100644
--- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems.indexer;
 
-import static frc.robot.subsystems.indexer.IndexConstants.FEEDUP_VOLT;
+import static frc.robot.subsystems.indexer.IndexerConstants.FEEDUP_VOLT;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
index 090e6cd5d..4536f6fdb 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
@@ -1,8 +1,8 @@
 package frc.robot.subsystems.indexer;
 
 import static frc.robot.Schematic.indexerFeedupCanId;
-import static frc.robot.subsystems.indexer.IndexConstants.ENCODER_FEEDUP_POSITION_CONVERSION;
-import static frc.robot.subsystems.indexer.IndexConstants.ENCODER_FEEDUP_VELOCITY_CONVERSION;
+import static frc.robot.subsystems.indexer.IndexerConstants.ENCODER_FEEDUP_POSITION_CONVERSION;
+import static frc.robot.subsystems.indexer.IndexerConstants.ENCODER_FEEDUP_VELOCITY_CONVERSION;
 
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;

From f86c87e116f71fb098851a828e7fcd46f28088c7 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Fri, 10 Apr 2026 22:05:31 -0400
Subject: [PATCH 030/102] indexer

---
 .../frc/robot/subsystems/indexer/IndexerConstants.java     | 7 +++++++
 1 file changed, 7 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java

diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
new file mode 100644
index 000000000..2125a44b1
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
@@ -0,0 +1,7 @@
+package frc.robot.subsystems.indexer;
+
+public class IndexerConstants {
+  public static final double ENCODER_FEEDUP_POSITION_CONVERSION = 1;
+  public static final double ENCODER_FEEDUP_VELOCITY_CONVERSION = 1;
+  public static final double FEEDUP_VOLT = 12;
+}

From 1fdf45eaad14b967b671bb961a1fb8447f49243a Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Fri, 10 Apr 2026 22:19:55 -0400
Subject: [PATCH 031/102] cycle2 autos

---
 .../deploy/pathplanner/paths/A-Cycle2.path    | 193 ++++++++++++++++++
 .../deploy/pathplanner/paths/B-Cycle1.path    |   6 +-
 .../deploy/pathplanner/paths/B-Cycle2.path    | 193 ++++++++++++++++++
 src/main/deploy/pathplanner/settings.json     |   2 +-
 4 files changed, 390 insertions(+), 4 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/A-Cycle2.path
 create mode 100644 src/main/deploy/pathplanner/paths/B-Cycle2.path

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
new file mode 100644
index 000000000..42a986a2f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -0,0 +1,193 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.256350626118068,
+        "y": 5.664652951699464
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6674363972882924,
+        "y": 5.6284350881968495
+      },
+      "isLocked": false,
+      "linkedName": "B-Cycle1-End"
+    },
+    {
+      "anchor": {
+        "x": 3.256350626118068,
+        "y": 7.254742397137747
+      },
+      "prevControl": {
+        "x": 2.1979715994142452,
+        "y": 6.347572517197855
+      },
+      "nextControl": {
+        "x": 3.5736065913462336,
+        "y": 7.526672441691426
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.4525328947368426,
+        "y": 7.254742397137747
+      },
+      "prevControl": {
+        "x": 5.8071316550071765,
+        "y": 7.778212679002352
+      },
+      "nextControl": {
+        "x": 7.57757028119644,
+        "y": 6.342250134115599
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.327521929824561,
+        "y": 4.409032894736844
+      },
+      "prevControl": {
+        "x": 6.863907746462161,
+        "y": 5.216361806741775
+      },
+      "nextControl": {
+        "x": 5.937085933334687,
+        "y": 3.8213770349498226
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.779396929824561,
+        "y": 4.553276315789475
+      },
+      "prevControl": {
+        "x": 5.799882977639703,
+        "y": 4.304117085888362
+      },
+      "nextControl": {
+        "x": 5.6990813534677445,
+        "y": 5.5301053915928176
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.779396929824561,
+        "y": 7.447760964912282
+      },
+      "prevControl": {
+        "x": 6.523025544615784,
+        "y": 6.993212709981212
+      },
+      "nextControl": {
+        "x": 5.527831315774364,
+        "y": 7.601532227331069
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1445504385964913,
+        "y": 7.447760964912282
+      },
+      "prevControl": {
+        "x": 3.619467665950238,
+        "y": 7.613424526324748
+      },
+      "nextControl": {
+        "x": 2.9084995834559355,
+        "y": 7.3654202472362
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.548344298245614,
+        "y": 6.284197368421054
+      },
+      "prevControl": {
+        "x": 1.9265215510296607,
+        "y": 7.001825662162521
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.1886792452830188,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.7264150943396226,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 2.0754716981132075,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 2.9433962264150932,
+      "rotationDegrees": -150.0
+    },
+    {
+      "waypointRelativePos": 3.9622641509434033,
+      "rotationDegrees": 80.0
+    },
+    {
+      "waypointRelativePos": 4.688679245283029,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 5.734276729559759,
+      "rotationDegrees": 90.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.526032315978456,
+      "maxWaypointRelativePos": 4.235188509874326,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 6,
+        "maxAngularVelocity": 540,
+        "maxAngularAcceleration": 720,
+        "nominalVoltage": 12,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4,
+    "maxAcceleration": 6,
+    "maxAngularVelocity": 540,
+    "maxAngularAcceleration": 720,
+    "nominalVoltage": 12,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -49.60011881716068
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -54.28065125914318
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index b508525b9..cd1edeea0 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -65,11 +65,11 @@
     {
       "anchor": {
         "x": 3.256350626118068,
-        "y": 2.4043470483005365
+        "y": 5.664652951699464
       },
       "prevControl": {
         "x": 4.3759033989266545,
-        "y": 2.339445438282647
+        "y": 5.599751341681575
       },
       "nextControl": null,
       "isLocked": false,
@@ -164,7 +164,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 50.71059313749959
+    "rotation": -54.28065125914318
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path
new file mode 100644
index 000000000..2e45e0179
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path
@@ -0,0 +1,193 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.256350626118068,
+        "y": 5.664652951699464
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6674363972882924,
+        "y": 5.700870815202079
+      },
+      "isLocked": false,
+      "linkedName": "B-Cycle1-End"
+    },
+    {
+      "anchor": {
+        "x": 3.256350626118068,
+        "y": 0.8142576028622535
+      },
+      "prevControl": {
+        "x": 2.1979715994142452,
+        "y": 1.721427482802146
+      },
+      "nextControl": {
+        "x": 3.5736065913462336,
+        "y": 0.5423275583085745
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.4525328947368426,
+        "y": 0.8142576028622535
+      },
+      "prevControl": {
+        "x": 5.8071316550071765,
+        "y": 0.2907873209976489
+      },
+      "nextControl": {
+        "x": 7.57757028119644,
+        "y": 1.7267498658844018
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.327521929824561,
+        "y": 3.6599671052631573
+      },
+      "prevControl": {
+        "x": 6.863907746462161,
+        "y": 2.852638193258226
+      },
+      "nextControl": {
+        "x": 5.937085933334687,
+        "y": 4.247622965050178
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.779396929824561,
+        "y": 3.515723684210526
+      },
+      "prevControl": {
+        "x": 5.799882977639703,
+        "y": 3.764882914111639
+      },
+      "nextControl": {
+        "x": 5.6990813534677445,
+        "y": 2.538894608407183
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.779396929824561,
+        "y": 0.6212390350877184
+      },
+      "prevControl": {
+        "x": 6.523025544615784,
+        "y": 1.0757872900187888
+      },
+      "nextControl": {
+        "x": 5.527831315774364,
+        "y": 0.46746777266893197
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1445504385964913,
+        "y": 0.6212390350877184
+      },
+      "prevControl": {
+        "x": 3.619467665950238,
+        "y": 0.4555754736752522
+      },
+      "nextControl": {
+        "x": 2.9084995834559355,
+        "y": 0.7035797527638007
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.548344298245614,
+        "y": 1.784802631578947
+      },
+      "prevControl": {
+        "x": 1.9265215510296607,
+        "y": 1.0671743378374803
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.1886792452830188,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.7264150943396226,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 2.0754716981132075,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 2.9433962264150932,
+      "rotationDegrees": 119.99999999999999
+    },
+    {
+      "waypointRelativePos": 3.9622641509434033,
+      "rotationDegrees": -100.0
+    },
+    {
+      "waypointRelativePos": 4.688679245283029,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 5.734276729559759,
+      "rotationDegrees": -90.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.526032315978456,
+      "maxWaypointRelativePos": 4.235188509874326,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 6.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 55.84030545433046
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -54.28065125914318
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 43ec2a33c..d5a531bdd 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -29,4 +29,4 @@
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
-}
\ No newline at end of file
+}

From f2ad47310b932856cc9e5310924fac487c6b35cb Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Fri, 10 Apr 2026 21:21:38 -0500
Subject: [PATCH 032/102] auto shake command

---
 src/main/java/frc/robot/subsystems/intake/Intake.java | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index d89a8c53c..1b582503e 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -38,10 +38,16 @@ public Command shakeAndIntake() {
             Commands.deadline(
                 extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), rollers.intake()),
             Commands.deadline(
-                extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), rollers.intake()))
+                extend.extendToAngle(FUNNEL_POS - SHAKE_POS_OFFSET), rollers.intake()))
         .withName("shakeAndIntake");
   }
-
+  public Command shake(){
+      return Commands.repeatingSequence(
+                  extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET),
+                  extend.extendToAngle(FUNNEL_POS - SHAKE_POS_OFFSET))
+          .withName("shake");
+    }
+  }
   // private because it doesn't have requirements and therefore it shouldn't be called beyond the
   // subsystem
   // itself

From e2b12fa895ce12c9317494c78e6318576ddd78a4 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Fri, 10 Apr 2026 21:24:13 -0500
Subject: [PATCH 033/102] intake fixed

---
 .../java/frc/robot/subsystems/intake/Intake.java | 16 ++++++----------
 1 file changed, 6 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 1b582503e..6ff303917 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -7,7 +7,6 @@
 import frc.robot.subsystems.intake.extend.IntakeExtend;
 import frc.robot.subsystems.intake.rollers.IntakeRollers;
 
-// TODO:rewrite this for v1.5
 public class Intake {
   private final IntakeRollers rollers;
   private final IntakeExtend extend;
@@ -41,14 +40,11 @@ public Command shakeAndIntake() {
                 extend.extendToAngle(FUNNEL_POS - SHAKE_POS_OFFSET), rollers.intake()))
         .withName("shakeAndIntake");
   }
-  public Command shake(){
-      return Commands.repeatingSequence(
-                  extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET),
-                  extend.extendToAngle(FUNNEL_POS - SHAKE_POS_OFFSET))
-          .withName("shake");
-    }
+
+  public Command shake() {
+    return Commands.repeatingSequence(
+            extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET),
+            extend.extendToAngle(FUNNEL_POS - SHAKE_POS_OFFSET))
+        .withName("shake");
   }
-  // private because it doesn't have requirements and therefore it shouldn't be called beyond the
-  // subsystem
-  // itself
 }

From f6e5d9fa730d1b66496324d06a260d0165b59b19 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Fri, 10 Apr 2026 22:48:09 -0400
Subject: [PATCH 034/102] cycle2 autos fixes

---
 .../deploy/pathplanner/paths/A-Cycle2.path    |  6 +-
 .../deploy/pathplanner/paths/B-Cycle1.path    | 10 +--
 .../deploy/pathplanner/paths/B-Cycle2.path    | 10 +--
 .../java/frc/robot/commands/auto/Autos.java   | 89 ++++++++++---------
 4 files changed, 59 insertions(+), 56 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index 42a986a2f..c7074f4f9 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -12,7 +12,7 @@
         "y": 5.6284350881968495
       },
       "isLocked": false,
-      "linkedName": "B-Cycle1-End"
+      "linkedName": "A-Cycle1-End"
     },
     {
       "anchor": {
@@ -181,13 +181,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -49.60011881716068
+    "rotation": -51.09865544424842
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -54.28065125914318
+    "rotation": -51.09865544424842
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index cd1edeea0..7c38315c8 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -64,12 +64,12 @@
     },
     {
       "anchor": {
-        "x": 3.256350626118068,
-        "y": 5.664652951699464
+        "x": 3.1752236135957066,
+        "y": 2.4043470483005365
       },
       "prevControl": {
-        "x": 4.3759033989266545,
-        "y": 5.599751341681575
+        "x": 4.2947763864042905,
+        "y": 2.339445438282647
       },
       "nextControl": null,
       "isLocked": false,
@@ -164,7 +164,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -54.28065125914318
+    "rotation": 51.952957468173956
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path
index 2e45e0179..4eecf8061 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.256350626118068,
-        "y": 5.664652951699464
+        "x": 3.1752236135957066,
+        "y": 2.4043470483005365
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.6674363972882924,
-        "y": 5.700870815202079
+        "x": 3.586309384765931,
+        "y": 2.4405649118031514
       },
       "isLocked": false,
       "linkedName": "B-Cycle1-End"
@@ -187,7 +187,7 @@
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -54.28065125914318
+    "rotation": 51.952957468173956
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index ba8319922..39a6d2d9c 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -151,27 +151,9 @@ public Command ppBCycleRight() {
 
   public Command ppB2CycleRightRegression() {
     return Commands.sequence(
-        Commands.runOnce(() -> drive.setPose(startBside.get())),
-        Commands.deadline(
-                drive.getAutonomousCommand("B-Cycle1"),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
-            .withTimeout(14.5),
-        Commands.deadline(
-            orchestrator.aimToHub().withTimeout(2.5),
-            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
-        Commands.parallel(
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                orchestrator.feedUp())
-            .withTimeout(2.5),
-        Commands.parallel(
-            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-            orchestrator.feedUp()))
-        .withTimeout(2.5)
-        .andThen(Commands.sequence(
             Commands.runOnce(() -> drive.setPose(startBside.get())),
             Commands.deadline(
-                    drive.getAutonomousCommand("B-Cycle2"),
+                    drive.getAutonomousCommand("B-Cycle1"),
                     intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
                 .withTimeout(14.5),
             Commands.deadline(
@@ -184,8 +166,28 @@ public Command ppB2CycleRightRegression() {
             Commands.parallel(
                 intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
                 orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                orchestrator.feedUp())));
+                orchestrator.feedUp()))
+        .withTimeout(2.5)
+        .andThen(
+            Commands.sequence(
+                Commands.runOnce(() -> drive.setPose(startBside.get())),
+                Commands.deadline(
+                        drive.getAutonomousCommand("B-Cycle2"),
+                        intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+                    .withTimeout(14.5),
+                Commands.deadline(
+                    orchestrator.aimToHub().withTimeout(2.5),
+                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
+                Commands.parallel(
+                        orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                        orchestrator.feedUp())
+                    .withTimeout(2.5),
+                Commands.parallel(
+                    intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                    orchestrator.feedUp())));
   }
+
   public Command ppA2CycleRightRegression() {
     return Commands.sequence(
             Commands.runOnce(() -> drive.setPose(startBside.get())),
@@ -205,23 +207,24 @@ public Command ppA2CycleRightRegression() {
                 orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
                 orchestrator.feedUp()))
         .withTimeout(2.5)
-        .andThen(Commands.sequence(
-            Commands.runOnce(() -> drive.setPose(startBside.get())),
-            Commands.deadline(
-                    drive.getAutonomousCommand("A-Cycle2"),
-                    intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
-                .withTimeout(14.5),
-            Commands.deadline(
-                orchestrator.aimToHub().withTimeout(2.5),
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
-            Commands.parallel(
+        .andThen(
+            Commands.sequence(
+                Commands.runOnce(() -> drive.setPose(startBside.get())),
+                Commands.deadline(
+                        drive.getAutonomousCommand("A-Cycle2"),
+                        intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+                    .withTimeout(14.5),
+                Commands.deadline(
+                    orchestrator.aimToHub().withTimeout(2.5),
+                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
+                Commands.parallel(
+                        orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                        orchestrator.feedUp())
+                    .withTimeout(2.5),
+                Commands.parallel(
+                    intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
                     orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                    orchestrator.feedUp())
-                .withTimeout(2.5),
-            Commands.parallel(
-                intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                orchestrator.feedUp())));
+                    orchestrator.feedUp())));
   }
 
   public Command ppBCycleRightRegression() {
@@ -338,7 +341,7 @@ public Command ADepot() {
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
                 .withTimeout(3.5),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
+            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 
   public Command ACCManuelAutoAlt() {
@@ -365,7 +368,7 @@ public Command ACCManuelAutoOverBump() {
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
                 .withTimeout(5),
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
             .withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
@@ -385,7 +388,7 @@ public Command ACCManuelAutoOverBump() {
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
                 .withTimeout(3.5),
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
+            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 
   public Command BCCManuelAuto() {
@@ -393,7 +396,7 @@ public Command BCCManuelAuto() {
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
                 .withTimeout(5),
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
             .withTimeout(5),
         rollers.stopIntakeCommand().withTimeout(0.05),
@@ -410,7 +413,7 @@ public Command BCCManuelAutoAlt() {
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
                 .withTimeout(5),
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
             .withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
@@ -429,7 +432,7 @@ public Command BCCManuelAutoOverBump() {
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
                 .withTimeout(5),
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
             .withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
@@ -449,6 +452,6 @@ public Command BCCManuelAutoOverBump() {
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
                 .withTimeout(3.5),
-                intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
+            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 }

From a0b14f6cfe18eb2ebd3248e56761e80da4199a67 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 09:26:18 -0500
Subject: [PATCH 035/102] updated vision constants, checked in sim

---
 src/main/java/frc/robot/Constants.java        |  2 +-
 src/main/java/frc/robot/RobotContainer.java   |  6 +--
 .../frc/robot/subsystems/drive/Drive.java     |  6 +--
 .../subsystems/vision/VisionConstants.java    | 40 +++++++------------
 4 files changed, 18 insertions(+), 36 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 922aca595..58fb36e35 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 956728d57..c09936056 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -107,8 +107,7 @@ public RobotContainer() {
                   drive::addVisionMeasurement,
                   new VisionIOPhotonVision(camera0Name, robotToCamera0),
                   new VisionIOPhotonVision(camera1Name, robotToCamera1),
-                  new VisionIOPhotonVision(camera2Name, robotToCamera2),
-                  new VisionIOPhotonVision(camera3Name, robotToCamera3));
+                  new VisionIOPhotonVision(camera2Name, robotToCamera2));
           leds = new Leds();
         }
         case ROBOT_2026_COMP -> {
@@ -124,8 +123,7 @@ public RobotContainer() {
                   drive::addVisionMeasurement,
                   new VisionIOPhotonVision(camera0Name, robotToCamera0),
                   new VisionIOPhotonVision(camera1Name, robotToCamera1),
-                  new VisionIOPhotonVision(camera2Name, robotToCamera2),
-                  new VisionIOPhotonVision(camera3Name, robotToCamera3));
+                  new VisionIOPhotonVision(camera2Name, robotToCamera2));
           leds = new Leds();
           shooter = new Shooter(new ShooterIOSparkMax());
           magicCarpet = new MagicCarpet(new MagicCarpetSparkMax());
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index e9b93a329..09d8613f1 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -98,7 +98,7 @@ public Drive(
 
   @Override
   public void periodic() {
-    // logCameraPositions(); // uncomment to show camera positions in advantage scope
+    //    logCameraPositions(); // uncomment to show camera positions in advantage scope
     odometryLock.lock(); // Prevents odometry updates while reading data
     gyroIO.updateInputs(gyroInputs);
     Logger.processInputs("Drive/Gyro", gyroInputs);
@@ -276,10 +276,6 @@ private void logCameraPositions() {
         "CameraPos/camera2position",
         new Pose3d(getPose().getX(), getPose().getY(), 0.0, new Rotation3d(getRotation()))
             .transformBy(VisionConstants.robotToCamera2));
-    Logger.recordOutput(
-        "CameraPos/camera3position",
-        new Pose3d(getPose().getX(), getPose().getY(), 0.0, new Rotation3d(getRotation()))
-            .transformBy(VisionConstants.robotToCamera3));
   }
 
   /** Returns the average velocity of the modules in rad/sec. */
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
index 3c7351771..f735e5419 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -26,47 +26,35 @@ public class VisionConstants {
   public static Transform3d robotToCamera0 =
       new Transform3d( // front camera
           new Translation3d(
-                  Units.inchesToMeters(8.779),
-                  Units.inchesToMeters(10.445),
-                  Units.inchesToMeters(27.152 + 1.75))
+                  Units.inchesToMeters(-8.931),
+                  Units.inchesToMeters(12.349),
+                  Units.inchesToMeters(15.2025))
               .rotateBy(RotationCorrection),
           new Rotation3d(
               Units.degreesToRadians(0.0),
-              Units.degreesToRadians(-25.2),
-              Units.degreesToRadians(0.0)));
+              Units.degreesToRadians(0.0),
+              Units.degreesToRadians(-158.771)));
   public static Transform3d robotToCamera1 =
       new Transform3d( // rear left camera
           new Translation3d(
-                  Units.inchesToMeters(9.562), // x
-                  Units.inchesToMeters(10.974), // y
-                  Units.inchesToMeters(17.035 + 1.75)) // z
+                  Units.inchesToMeters(8.931), // x
+                  Units.inchesToMeters(12.349), // y
+                  Units.inchesToMeters(15.2025)) // z
               .rotateBy(RotationCorrection),
           new Rotation3d(
               Units.degreesToRadians(0.0),
-              Units.degreesToRadians(-11.32),
-              Units.degreesToRadians(155.3)));
+              Units.degreesToRadians(0.0),
+              Units.degreesToRadians(-201.229)));
 
   public static Transform3d robotToCamera2 =
       new Transform3d( // rear right camera
           new Translation3d(
-                  Units.inchesToMeters(-9.562),
-                  Units.inchesToMeters(10.974),
-                  Units.inchesToMeters(17.035 + 1.75))
-              .rotateBy(RotationCorrection),
-          new Rotation3d(
-              Units.degreesToRadians(0),
-              Units.degreesToRadians(-11.32),
-              Units.degreesToRadians(-155.3)));
-
-  public static Transform3d robotToCamera3 =
-      new Transform3d( // left camera
-          new Translation3d(
-                  Units.inchesToMeters(9.669),
-                  Units.inchesToMeters(10.961),
-                  Units.inchesToMeters(24.656 + 1.75))
+                  Units.inchesToMeters(11.425),
+                  Units.inchesToMeters(-2.361),
+                  Units.inchesToMeters(20.5805))
               .rotateBy(RotationCorrection),
           new Rotation3d(
-              Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(75)));
+              Units.degreesToRadians(0), Units.degreesToRadians(-30), Units.degreesToRadians(0)));
 
   // Basic filtering thresholds
   public static double maxAmbiguity = 0.3;

From 20b9b7a1e9b1849e63720edcf09c96190dee90b0 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 09:26:55 -0500
Subject: [PATCH 036/102] remove camera std dev factors for camera 3

---
 src/main/java/frc/robot/subsystems/vision/VisionConstants.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
index f735e5419..109cc6887 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -73,6 +73,5 @@ public class VisionConstants {
         1.0, // Camera 0
         1.0, // Camera 1
         1.0, // CAMERA 2
-        1.0 // Camera 3
       };
 }

From baba3dfa86a5bcd7a747487a5ad63f4e1e40c8da Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 09:28:34 -0500
Subject: [PATCH 037/102] Switch back to normal robot

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 58fb36e35..922aca595 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";

From 46bbc6126262e12503bcfa619b26a8c30e82cbd7 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 09:42:00 -0500
Subject: [PATCH 038/102] remove robot type override

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 58fb36e35..922aca595 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";

From f985c6d8bb92ccb545ace0f2aab15aa5336dfc12 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 09:48:34 -0500
Subject: [PATCH 039/102] feedup logic changes

---
 src/main/java/frc/robot/Orchestrator.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 3628e595c..75ed80358 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -243,6 +243,7 @@ public Command feedUp() {
             Commands.parallel(magicCarpet.run(), indexer.run())
                 .until(() -> !RobotState.getInstance().shooterAtSpeed))
         .onlyIf(() -> shooter.getSetpoint() > 0)
+        .onlyIf(() -> RobotState.getInstance().shooterAtSpeed)
         .onlyWhile(() -> shooter.getSetpoint() > 0)
         .withName("feedUp");
   }

From 975fd1879cf6b9f3558d5fb1d58c2507a0ebd5f1 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 09:57:17 -0500
Subject: [PATCH 040/102] intake safety feature (intake can't run when in)

---
 src/main/java/frc/robot/subsystems/intake/Intake.java        | 5 ++++-
 .../java/frc/robot/subsystems/intake/IntakeConstants.java    | 1 +
 .../frc/robot/subsystems/intake/extend/IntakeExtend.java     | 5 +++++
 3 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 6ff303917..ba57e447d 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.intake.extend.IntakeExtend;
 import frc.robot.subsystems.intake.rollers.IntakeRollers;
+import java.util.function.DoubleSupplier;
 
 public class Intake {
   private final IntakeRollers rollers;
@@ -31,7 +32,9 @@ public Command holdAngleAndIntake(double angle) {
   }
 
   // Jack's Chugga Chugga mode
-
+  public Command runIntakeMotor(){
+    return rollers.intake().onlyWhile(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS + SAFETY_TOLERANCE);
+  }
   public Command shakeAndIntake() {
     return Commands.repeatingSequence(
             Commands.deadline(
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index fc514cf86..15eecb470 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -28,6 +28,7 @@ public class IntakeConstants {
   // shake around the funnel pos by this much
   public static final double SHAKE_POS_OFFSET = 1;
   public static final double COLLAPSE_POS = 4.0;
+  public static final double SAFETY_TOLERANCE = 2;
   public static final double POSITION_TOLERANCE = 7;
 
   public static final double INTAKE_EXTEND_MOTOR_CURRENT_LIMIT = 20;
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index cb9e8a46f..e0c8dd46c 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -12,6 +12,7 @@
 import frc.robot.RobotState;
 import frc.robot.RobotState.IntakePosition;
 import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 public class IntakeExtend extends SubsystemBase {
@@ -65,6 +66,10 @@ public boolean isHopperCollapsed() {
     return io.isCollapsed();
   }
 
+  public DoubleSupplier getAngle(){
+    return () -> inputs.getExtendPos;
+  }
+
   private boolean isStallingExtend() {
     // For our volts, we are not getting the right velocity
     return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts < -0.01;

From 00873d1d8241afdddcc5ff64d3c6b0c5aeb0324d Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 10:05:25 -0500
Subject: [PATCH 041/102] feat: intake safety feature to stop running roller
 motors when in the collapse position

---
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../frc/robot/subsystems/intake/Intake.java   | 19 +++++++++++--------
 .../intake/extend/IntakeExtend.java           |  2 +-
 3 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c09936056..8e513fe0e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -361,7 +361,7 @@ private void configureButtonBindings() {
         .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
 
     //     VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
-    driverController.leftTrigger(0.2).toggleOnTrue(intakeRollers.intake());
+    driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
 
     driverController
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index ba57e447d..468aaec25 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -6,7 +6,6 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.intake.extend.IntakeExtend;
 import frc.robot.subsystems.intake.rollers.IntakeRollers;
-import java.util.function.DoubleSupplier;
 
 public class Intake {
   private final IntakeRollers rollers;
@@ -18,7 +17,7 @@ public Intake(IntakeRollers rollers, IntakeExtend extend) {
   }
 
   public Command extendToAngleAndIntake(double angle) {
-    return Commands.parallel(extend.extendToAngle(angle), rollers.intake())
+    return Commands.parallel(extend.extendToAngle(angle), runIntakeMotor())
         .withName("extendToAngleAndIntake");
   }
 
@@ -27,20 +26,24 @@ public Command extendToAngle(double angle) {
   }
 
   public Command holdAngleAndIntake(double angle) {
-    return Commands.parallel(extend.holdAngle(angle), rollers.intake())
+    return Commands.parallel(extend.holdAngle(angle), runIntakeMotor())
         .withName("holdAngleAndIntake");
   }
 
-  // Jack's Chugga Chugga mode
-  public Command runIntakeMotor(){
-    return rollers.intake().onlyWhile(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS + SAFETY_TOLERANCE);
+  public Command runIntakeMotor() {
+    return rollers
+        .intake()
+        .onlyWhile(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS - SAFETY_TOLERANCE)
+        .onlyIf(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS - SAFETY_TOLERANCE);
   }
+
+  // Jack's Chugga Chugga mode
   public Command shakeAndIntake() {
     return Commands.repeatingSequence(
             Commands.deadline(
-                extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), rollers.intake()),
+                extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), runIntakeMotor()),
             Commands.deadline(
-                extend.extendToAngle(FUNNEL_POS - SHAKE_POS_OFFSET), rollers.intake()))
+                extend.extendToAngle(FUNNEL_POS - SHAKE_POS_OFFSET), runIntakeMotor()))
         .withName("shakeAndIntake");
   }
 
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index e0c8dd46c..d693d9cf0 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -66,7 +66,7 @@ public boolean isHopperCollapsed() {
     return io.isCollapsed();
   }
 
-  public DoubleSupplier getAngle(){
+  public DoubleSupplier getAngle() {
     return () -> inputs.getExtendPos;
   }
 

From 80eae254dc766362b93dbbd9c2135caa76b20255 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 10:09:18 -0500
Subject: [PATCH 042/102] add a comment to explain ambiguous constants

---
 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 15eecb470..b30e2f1d0 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -28,6 +28,7 @@ public class IntakeConstants {
   // shake around the funnel pos by this much
   public static final double SHAKE_POS_OFFSET = 1;
   public static final double COLLAPSE_POS = 4.0;
+  // How close we need to be to collapse position to stop the intake rollers
   public static final double SAFETY_TOLERANCE = 2;
   public static final double POSITION_TOLERANCE = 7;
 

From 3ab6c2aefd6977f5c3ed82cbbe6097deeef139dd Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 11:10:14 -0400
Subject: [PATCH 043/102] Shrink pathplanner autos

---
 .../java/frc/robot/commands/auto/Autos.java   | 95 +++++--------------
 1 file changed, 22 insertions(+), 73 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 39a6d2d9c..3027a7064 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -117,11 +117,11 @@ public Command Bummmmpar() {
             () -> AllianceFlipUtil.apply(new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(0)))));
   }
 
-  public Command ppACycleLeft() {
+  private Command ppCycle(String path) {
     return Commands.sequence(
         Commands.runOnce(() -> drive.setPose(startAside.get())),
         Commands.deadline(
-                drive.getAutonomousCommand("A-Cycle-LeftSweep"),
+                drive.getAutonomousCommand(path),
                 intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
                 orchestrator.spinUpShooterHub())
             .withTimeout(14.5),
@@ -133,27 +133,19 @@ public Command ppACycleLeft() {
             orchestrator.feedUp()));
   }
 
+  public Command ppACycleLeft() {
+    return ppCycle("A-Cycle-LeftSweep");
+  }
+
   public Command ppBCycleRight() {
-    return Commands.sequence(
-        Commands.runOnce(() -> drive.setPose(startBside.get())),
-        Commands.deadline(
-                drive.getAutonomousCommand("B-Cycle-RightSweep"),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
-                orchestrator.spinUpShooterHub())
-            .withTimeout(14.5),
-        orchestrator.aimToHub().withTimeout(2.5),
-        Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5),
-        Commands.parallel(
-            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-            orchestrator.spinUpShooter(1214),
-            orchestrator.feedUp()));
+    return ppCycle("B-Cycle-RightSweep");
   }
 
-  public Command ppB2CycleRightRegression() {
+  private Command pp2CycleRegression(String path1, String path2) {
     return Commands.sequence(
             Commands.runOnce(() -> drive.setPose(startBside.get())),
             Commands.deadline(
-                    drive.getAutonomousCommand("B-Cycle1"),
+                    drive.getAutonomousCommand(path1),
                     intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
                 .withTimeout(14.5),
             Commands.deadline(
@@ -172,7 +164,7 @@ public Command ppB2CycleRightRegression() {
             Commands.sequence(
                 Commands.runOnce(() -> drive.setPose(startBside.get())),
                 Commands.deadline(
-                        drive.getAutonomousCommand("B-Cycle2"),
+                        drive.getAutonomousCommand(path2),
                         intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
                     .withTimeout(14.5),
                 Commands.deadline(
@@ -188,50 +180,19 @@ public Command ppB2CycleRightRegression() {
                     orchestrator.feedUp())));
   }
 
+  public Command ppB2CycleRightRegression() {
+    return pp2CycleRegression("B-Cycle1", "B-Cycle2");
+  }
+
   public Command ppA2CycleRightRegression() {
-    return Commands.sequence(
-            Commands.runOnce(() -> drive.setPose(startBside.get())),
-            Commands.deadline(
-                    drive.getAutonomousCommand("A-Cycle1"),
-                    intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
-                .withTimeout(14.5),
-            Commands.deadline(
-                orchestrator.aimToHub().withTimeout(2.5),
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
-            Commands.parallel(
-                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                    orchestrator.feedUp())
-                .withTimeout(2.5),
-            Commands.parallel(
-                intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                orchestrator.feedUp()))
-        .withTimeout(2.5)
-        .andThen(
-            Commands.sequence(
-                Commands.runOnce(() -> drive.setPose(startBside.get())),
-                Commands.deadline(
-                        drive.getAutonomousCommand("A-Cycle2"),
-                        intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
-                    .withTimeout(14.5),
-                Commands.deadline(
-                    orchestrator.aimToHub().withTimeout(2.5),
-                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
-                Commands.parallel(
-                        orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                        orchestrator.feedUp())
-                    .withTimeout(2.5),
-                Commands.parallel(
-                    intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                    orchestrator.feedUp())));
+    return pp2CycleRegression("A-Cycle1", "A-Cycle2");
   }
 
-  public Command ppBCycleRightRegression() {
+  private Command ppCycleRegression(String path) {
     return Commands.sequence(
         Commands.runOnce(() -> drive.setPose(startBside.get())),
         Commands.deadline(
-                drive.getAutonomousCommand("B-Cycle-RightSweep"),
+                drive.getAutonomousCommand(path),
                 intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
             .withTimeout(14.5),
         Commands.deadline(
@@ -247,24 +208,12 @@ public Command ppBCycleRightRegression() {
             orchestrator.feedUp()));
   }
 
+  public Command ppBCycleRightRegression() {
+    return ppCycleRegression("B-Cycle-RightSweep");
+  }
+
   public Command ppACycleLeftRegression() {
-    return Commands.sequence(
-        Commands.runOnce(() -> drive.setPose(startAside.get())),
-        Commands.deadline(
-                drive.getAutonomousCommand("A-Cycle-LeftSweep"),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
-            .withTimeout(14.5),
-        Commands.deadline(
-            orchestrator.aimToHub().withTimeout(2.5),
-            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
-        Commands.parallel(
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                orchestrator.feedUp())
-            .withTimeout(2.5),
-        Commands.parallel(
-            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-            orchestrator.feedUp()));
+    return ppCycleRegression("A-Cycle-LeftSweep");
   }
 
   public Command EightBalls() {

From 85a0e14ec03fda6137365e1b7f9d4d46fc5a8296 Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 11:51:49 -0400
Subject: [PATCH 044/102] Lots more shrinking

---
 .../java/frc/robot/commands/auto/Autos.java   | 284 ++++++++----------
 1 file changed, 118 insertions(+), 166 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 3027a7064..9d8adb007 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -12,6 +12,7 @@
 import frc.robot.subsystems.intake.IntakeConstants;
 import frc.robot.subsystems.intake.rollers.IntakeRollers;
 import frc.robot.subsystems.shooter.Shooter;
+import java.util.List;
 import java.util.function.Supplier;
 
 public class Autos {
@@ -117,6 +118,8 @@ public Command Bummmmpar() {
             () -> AllianceFlipUtil.apply(new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(0)))));
   }
 
+
+  // Helper functions that extract common code for pathplanner atuos
   private Command ppCycle(String path) {
     return Commands.sequence(
         Commands.runOnce(() -> drive.setPose(startAside.get())),
@@ -133,61 +136,6 @@ private Command ppCycle(String path) {
             orchestrator.feedUp()));
   }
 
-  public Command ppACycleLeft() {
-    return ppCycle("A-Cycle-LeftSweep");
-  }
-
-  public Command ppBCycleRight() {
-    return ppCycle("B-Cycle-RightSweep");
-  }
-
-  private Command pp2CycleRegression(String path1, String path2) {
-    return Commands.sequence(
-            Commands.runOnce(() -> drive.setPose(startBside.get())),
-            Commands.deadline(
-                    drive.getAutonomousCommand(path1),
-                    intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
-                .withTimeout(14.5),
-            Commands.deadline(
-                orchestrator.aimToHub().withTimeout(2.5),
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
-            Commands.parallel(
-                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                    orchestrator.feedUp())
-                .withTimeout(2.5),
-            Commands.parallel(
-                intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                orchestrator.feedUp()))
-        .withTimeout(2.5)
-        .andThen(
-            Commands.sequence(
-                Commands.runOnce(() -> drive.setPose(startBside.get())),
-                Commands.deadline(
-                        drive.getAutonomousCommand(path2),
-                        intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
-                    .withTimeout(14.5),
-                Commands.deadline(
-                    orchestrator.aimToHub().withTimeout(2.5),
-                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
-                Commands.parallel(
-                        orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                        orchestrator.feedUp())
-                    .withTimeout(2.5),
-                Commands.parallel(
-                    intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                    orchestrator.feedUp())));
-  }
-
-  public Command ppB2CycleRightRegression() {
-    return pp2CycleRegression("B-Cycle1", "B-Cycle2");
-  }
-
-  public Command ppA2CycleRightRegression() {
-    return pp2CycleRegression("A-Cycle1", "A-Cycle2");
-  }
-
   private Command ppCycleRegression(String path) {
     return Commands.sequence(
         Commands.runOnce(() -> drive.setPose(startBside.get())),
@@ -208,6 +156,31 @@ private Command ppCycleRegression(String path) {
             orchestrator.feedUp()));
   }
 
+  private Command pp2CycleRegression(String path1, String path2) {
+    return ppCycleRegression(path1)
+        .withTimeout(2.5)
+        .andThen(
+            ppCycleRegression(path2));
+  }
+
+  // Pathplanner autos using the helper functions
+
+  public Command ppACycleLeft() {
+    return ppCycle("A-Cycle-LeftSweep");
+  }
+
+  public Command ppBCycleRight() {
+    return ppCycle("B-Cycle-RightSweep");
+  }
+
+  public Command ppB2CycleRightRegression() {
+    return pp2CycleRegression("B-Cycle1", "B-Cycle2");
+  }
+
+  public Command ppA2CycleRightRegression() {
+    return pp2CycleRegression("A-Cycle1", "A-Cycle2");
+  }
+
   public Command ppBCycleRightRegression() {
     return ppCycleRegression("B-Cycle-RightSweep");
   }
@@ -216,94 +189,51 @@ public Command ppACycleLeftRegression() {
     return ppCycleRegression("A-Cycle-LeftSweep");
   }
 
-  public Command EightBalls() {
-    return Commands.sequence(
-        Commands.deadline(
-            orchestrator.driveToHub().withTimeout(3.0), orchestrator.spinUpShooterHub()),
-        Commands.parallel(orchestrator.spinUpShooterHub(), orchestrator.feedUp()));
+  // Misc helper functions for manual autos
+  private Command shootPullIntake() {
+    return Commands.parallel(
+        orchestrator.spinUpShooterHub(),
+        orchestrator.feedUp(),
+        Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0)));
   }
 
-  public Command ACCManuelAuto() {
+  List<Pose2d> getPoses(boolean isA) {
+    if (isA) {
+      return List.of(intakeSimplePoseA.get(), overBumpAllianceAltPoseA.get(), shootFromCornerPoseA.get());
+    }
+    return List.of(intakeSimplePoseB.get(), overBumpAllianceAltPoseB.get(), shootFromCornerPoseB.get());
+  }
+
+  // Helper functions that extract common code for manual autos
+  private Command ccManualAuto(boolean isA) {
+    List<Pose2d> poses = getPoses(isA);
+
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0)))
                 .withTimeout(5),
-            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAlliancePoseA.get()))
+            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1)))
             .withTimeout(5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
-        Commands.parallel(
-            orchestrator.spinUpShooterHub(),
-            orchestrator.feedUp(),
-            Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))));
+        shootPullIntake());
   }
 
-  public Command ACCManuelImprovedComplexIntake() {
-    return Commands.sequence(
-        Commands.deadline(
-            Commands.sequence(
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpNeutralPoseA.get()))
-                    .withTimeout(2),
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexFirstPoseA.get())),
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexSecondPoseA.get()))
-                    .withTimeout(1.9),
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexThirdPoseA.get()))
-                    .withTimeout(3),
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpNeutralPoseA.get()))),
-            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
-            .withTimeout(3.5),
-        rollers.stopIntakeCommand().withTimeout(0.05),
-        Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
-                .withTimeout(4),
-            orchestrator.spinUpShooter(1240)),
-        Commands.parallel(
-            orchestrator.spinUpShooter(1240),
-            orchestrator.feedUp(),
-            Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))));
-  }
+  private Command ccManualAutoAlt(boolean isA) {
+    List<Pose2d> poses = getPoses(isA);
 
-  public Command ADepot() {
-    return Commands.sequence(
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPoseStopPoint.get()))
-            .withTimeout(2),
-        Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPose.get())).withTimeout(3.5),
-            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        rollers.stopIntakeCommand().withTimeout(0.05),
+   return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
-                .withTimeout(2),
-            orchestrator.spinUpShooter(1250)),
-        Commands.deadline(
-            Commands.waitSeconds(5),
-            orchestrator.spinUpShooter(1250),
-            orchestrator.feedUp(),
-            Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
-        rollers.stopIntakeCommand().withTimeout(0.05),
-        shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
-            .withTimeout(2),
-        Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
-                .withTimeout(3.5),
-            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
-  }
-
-  public Command ACCManuelAutoAlt() {
-    return Commands.sequence(
-        Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0)))
                 .withTimeout(5),
-            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
+            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1)))
             .withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(2)))
                 .withTimeout(2),
             orchestrator.spinUpShooter(1240)),
         Commands.parallel(
@@ -312,17 +242,19 @@ public Command ACCManuelAutoAlt() {
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))));
   }
 
-  public Command ACCManuelAutoOverBump() {
+  private Command ccManualAutoOverBump(boolean isA) {
+    List<Pose2d> poses = getPoses(isA);
+
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0)))
                 .withTimeout(5),
             intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1)))
             .withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(2)))
                 .withTimeout(2),
             orchestrator.spinUpShooter(1250)),
         Commands.deadline(
@@ -332,61 +264,80 @@ public Command ACCManuelAutoOverBump() {
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
         rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1)))
             .withTimeout(2),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0)))
                 .withTimeout(3.5),
             intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 
+  // Manual autos using the helper functions
+  public Command ACCManuelAuto() {
+    return ccManualAuto(true);
+  }
+
   public Command BCCManuelAuto() {
+    return ccManualAuto(false);
+  }
+
+  public Command ACCManuelAutoAlt() {
+    return ccManualAutoAlt(true);
+  }
+
+  public Command BCCManuelAutoAlt() {
+    return ccManualAutoAlt(false);
+  }
+
+  public Command ACCManuelAutoOverBump() {
+    return ccManualAutoOverBump(true);
+  }
+
+  public Command BCCManuelAutoOverBump() {
+    return ccManualAutoOverBump(false);
+  }
+
+  // Other manual autos
+  public Command EightBalls() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
-                .withTimeout(5),
-            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
-            .withTimeout(5),
-        rollers.stopIntakeCommand().withTimeout(0.05),
-        Commands.deadline(
-            orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
-        Commands.parallel(
-            orchestrator.spinUpShooterHub(),
-            orchestrator.feedUp(),
-            Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))));
+        orchestrator.driveToHub().withTimeout(3.0), orchestrator.spinUpShooterHub()),
+        Commands.parallel(orchestrator.spinUpShooterHub(), orchestrator.feedUp()));
   }
 
-  public Command BCCManuelAutoAlt() {
+  public Command ACCManuelImprovedComplexIntake() {
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
-                .withTimeout(5),
-            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
+            Commands.sequence(
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpNeutralPoseA.get()))
+                    .withTimeout(2),
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexFirstPoseA.get())),
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexSecondPoseA.get()))
+                    .withTimeout(1.9),
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexThirdPoseA.get()))
+                    .withTimeout(3),
+                new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpNeutralPoseA.get()))),
+            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
             .withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseB.get()))
-                .withTimeout(2),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
+                .withTimeout(4),
             orchestrator.spinUpShooter(1240)),
-        Commands.parallel(
-            orchestrator.spinUpShooter(1240),
-            orchestrator.feedUp(),
-            Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))));
+        shootPullIntake());
   }
 
-  public Command BCCManuelAutoOverBump() {
+  public Command ADepot() {
     return Commands.sequence(
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPoseStopPoint.get()))
+            .withTimeout(2),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
-                .withTimeout(5),
-            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
-            .withTimeout(3.5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPose.get())).withTimeout(3.5),
+            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseB.get()))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
                 .withTimeout(2),
             orchestrator.spinUpShooter(1250)),
         Commands.deadline(
@@ -396,11 +347,12 @@ public Command BCCManuelAutoOverBump() {
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
         rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseB.get()))
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
             .withTimeout(2),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseB.get()))
-                .withTimeout(3.5),
-            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+            .withTimeout(3.5),
+        intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
+
 }

From 4e04441ebd54fff8ac5f8d62e971d9e98a358621 Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 11:54:06 -0400
Subject: [PATCH 045/102] Formatting

---
 .../java/frc/robot/commands/auto/Autos.java   | 53 +++++++------------
 1 file changed, 20 insertions(+), 33 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 9d8adb007..9d3b692dc 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -118,7 +118,6 @@ public Command Bummmmpar() {
             () -> AllianceFlipUtil.apply(new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(0)))));
   }
 
-
   // Helper functions that extract common code for pathplanner atuos
   private Command ppCycle(String path) {
     return Commands.sequence(
@@ -157,10 +156,7 @@ private Command ppCycleRegression(String path) {
   }
 
   private Command pp2CycleRegression(String path1, String path2) {
-    return ppCycleRegression(path1)
-        .withTimeout(2.5)
-        .andThen(
-            ppCycleRegression(path2));
+    return ppCycleRegression(path1).withTimeout(2.5).andThen(ppCycleRegression(path2));
   }
 
   // Pathplanner autos using the helper functions
@@ -199,9 +195,11 @@ private Command shootPullIntake() {
 
   List<Pose2d> getPoses(boolean isA) {
     if (isA) {
-      return List.of(intakeSimplePoseA.get(), overBumpAllianceAltPoseA.get(), shootFromCornerPoseA.get());
+      return List.of(
+          intakeSimplePoseA.get(), overBumpAllianceAltPoseA.get(), shootFromCornerPoseA.get());
     }
-    return List.of(intakeSimplePoseB.get(), overBumpAllianceAltPoseB.get(), shootFromCornerPoseB.get());
+    return List.of(
+        intakeSimplePoseB.get(), overBumpAllianceAltPoseB.get(), shootFromCornerPoseB.get());
   }
 
   // Helper functions that extract common code for manual autos
@@ -210,11 +208,9 @@ private Command ccManualAuto(boolean isA) {
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0)))
-                .withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(5),
             intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1)))
-            .withTimeout(5),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
@@ -224,17 +220,14 @@ private Command ccManualAuto(boolean isA) {
   private Command ccManualAutoAlt(boolean isA) {
     List<Pose2d> poses = getPoses(isA);
 
-   return Commands.sequence(
+    return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0)))
-                .withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(5),
             intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1)))
-            .withTimeout(3.5),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(2)))
-                .withTimeout(2),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(2))).withTimeout(2),
             orchestrator.spinUpShooter(1240)),
         Commands.parallel(
             orchestrator.spinUpShooter(1240),
@@ -247,15 +240,12 @@ private Command ccManualAutoOverBump(boolean isA) {
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0)))
-                .withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(5),
             intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1)))
-            .withTimeout(3.5),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(2)))
-                .withTimeout(2),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(2))).withTimeout(2),
             orchestrator.spinUpShooter(1250)),
         Commands.deadline(
             Commands.waitSeconds(5),
@@ -264,11 +254,9 @@ private Command ccManualAutoOverBump(boolean isA) {
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
         rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1)))
-            .withTimeout(2),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(2),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0)))
-                .withTimeout(3.5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(3.5),
             intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 
@@ -301,7 +289,7 @@ public Command BCCManuelAutoOverBump() {
   public Command EightBalls() {
     return Commands.sequence(
         Commands.deadline(
-        orchestrator.driveToHub().withTimeout(3.0), orchestrator.spinUpShooterHub()),
+            orchestrator.driveToHub().withTimeout(3.0), orchestrator.spinUpShooterHub()),
         Commands.parallel(orchestrator.spinUpShooterHub(), orchestrator.feedUp()));
   }
 
@@ -350,9 +338,8 @@ public Command ADepot() {
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
             .withTimeout(2),
         Commands.deadline(
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
-            .withTimeout(3.5),
-        intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
+                .withTimeout(3.5),
+            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
-
 }

From 42f4276ce4f193747473c84750f38245f1902e58 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 10:59:51 -0500
Subject: [PATCH 046/102] current progress on fixing the shoot from zone
 command

---
 src/main/java/frc/robot/Constants.java      |  2 +-
 src/main/java/frc/robot/Orchestrator.java   | 73 +++++++++++----------
 src/main/java/frc/robot/RobotContainer.java |  4 +-
 3 files changed, 42 insertions(+), 37 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 922aca595..58fb36e35 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";
diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 75ed80358..dba9ae581 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -10,6 +10,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.lib.utils.AllianceFlipUtil;
 import frc.robot.FieldConstants.Hub;
@@ -25,6 +27,7 @@
 import frc.robot.util.ShooterLeadCompensator;
 import frc.robot.util.Zone;
 import frc.robot.util.Zone.Tuple2d;
+import java.util.Map;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
@@ -126,45 +129,47 @@ public ZoneId getCurrentZone() {
   }
 
   public Command executeCurrentZoneLogic() {
-    double allianceY = AllianceFlipUtil.applyY(drive.getPose().getY());
-
-    return switch (getCurrentZone()) {
-      case ZONE_1 ->
-          new RotateToOrientation(
-              drive,
-              AllianceFlipUtil.apply(
-                  new Pose2d(4.621539115905762, 4.040013313293457, new Rotation2d())));
-      case ZONE_2 -> {
-        if (allianceY > 4.029185771942139) {
-          yield new RotateToOrientation(
-              drive,
-              AllianceFlipUtil.apply(
-                  new Pose2d(4.415811061859131, 5.599213600158691, new Rotation2d())));
-        } else {
-          yield new RotateToOrientation(
-              drive,
-              AllianceFlipUtil.apply(
-                  new Pose2d(4.415811061859131, 2.534952402114868, new Rotation2d())));
-        }
-      }
-      case ZONE_3 ->
-          new RotateToOrientation(
-              drive,
-              AllianceFlipUtil.apply(
-                  new Pose2d(11.334761619567871, 5.599213600158691, new Rotation2d())));
-      case ZONE_4 ->
-          new RotateToOrientation(
-              drive,
-              AllianceFlipUtil.apply(
-                  new Pose2d(11.334761619567871, 2.534952402114868, new Rotation2d())));
-      case NONE -> Commands.none();
-    };
+    DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY());
+
+    return new SelectCommand<>(
+        Map.ofEntries(
+            Map.entry(
+                ZoneId.ZONE_1,
+                new RotateToOrientation(
+                    drive,
+                    AllianceFlipUtil.apply(
+                        new Pose2d(4.621539115905762, 4.040013313293457, new Rotation2d())))),
+            Map.entry(
+                ZoneId.ZONE_2,
+                new ConditionalCommand(
+                    new RotateToOrientation(
+                        drive,
+                        AllianceFlipUtil.apply(
+                            new Pose2d(4.415811061859131, 5.599213600158691, new Rotation2d()))),
+                    new RotateToOrientation(
+                        drive,
+                        AllianceFlipUtil.apply(
+                            new Pose2d(4.415811061859131, 2.534952402114868, new Rotation2d()))),
+                    () -> allianceY.getAsDouble() > 4.029185771942139)),
+            Map.entry(
+                ZoneId.ZONE_3,
+                new RotateToOrientation(
+                    drive,
+                    AllianceFlipUtil.apply(
+                        new Pose2d(11.334761619567871, 5.599213600158691, new Rotation2d())))),
+            Map.entry(
+                ZoneId.ZONE_4,
+                new RotateToOrientation(
+                    drive,
+                    AllianceFlipUtil.apply(
+                        new Pose2d(11.334761619567871, 2.534952402114868, new Rotation2d()))))),
+        this::getCurrentZone);
   }
 
   public Command zoneBasedShoot() {
     return Commands.deadline(
             executeCurrentZoneLogic(),
-            indexer.run(),
+            feedUp(),
             Commands.run(
                 () -> Logger.recordOutput("Orchestrator/CurrentZone", getCurrentZone().name())))
         .withName("zoneBasedShoot");
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 8e513fe0e..1e02f3685 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -342,7 +342,6 @@ private void configureButtonBindings() {
     new Trigger(() -> driverController.getHID().getPOV() != -1)
         .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV()));
     driverController.x().toggleOnTrue(orchestrator.aimToHub());
-
     driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
     driverController
         .leftBumper()
@@ -361,7 +360,8 @@ private void configureButtonBindings() {
         .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
 
     //     VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
-    driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
+    //    driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
+    driverController.leftTrigger(0.2).toggleOnTrue(orchestrator.zoneBasedShoot());
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
 
     driverController

From 7e8780c8b40a1854796d2fe843f6fbc1c193cc00 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 11:09:02 -0500
Subject: [PATCH 047/102] fixed shoot from zone command

---
 src/main/java/frc/robot/Orchestrator.java | 29 +++++++++++++++--------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index dba9ae581..5417a9107 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -137,32 +137,39 @@ public Command executeCurrentZoneLogic() {
                 ZoneId.ZONE_1,
                 new RotateToOrientation(
                     drive,
-                    AllianceFlipUtil.apply(
-                        new Pose2d(4.621539115905762, 4.040013313293457, new Rotation2d())))),
+                    () ->
+                        AllianceFlipUtil.apply(
+                            new Pose2d(4.621539115905762, 4.040013313293457, new Rotation2d())))),
             Map.entry(
                 ZoneId.ZONE_2,
                 new ConditionalCommand(
                     new RotateToOrientation(
                         drive,
-                        AllianceFlipUtil.apply(
-                            new Pose2d(4.415811061859131, 5.599213600158691, new Rotation2d()))),
+                        () ->
+                            AllianceFlipUtil.apply(
+                                new Pose2d(
+                                    4.415811061859131, 5.599213600158691, new Rotation2d()))),
                     new RotateToOrientation(
                         drive,
-                        AllianceFlipUtil.apply(
-                            new Pose2d(4.415811061859131, 2.534952402114868, new Rotation2d()))),
+                        () ->
+                            AllianceFlipUtil.apply(
+                                new Pose2d(
+                                    4.415811061859131, 2.534952402114868, new Rotation2d()))),
                     () -> allianceY.getAsDouble() > 4.029185771942139)),
             Map.entry(
                 ZoneId.ZONE_3,
                 new RotateToOrientation(
                     drive,
-                    AllianceFlipUtil.apply(
-                        new Pose2d(11.334761619567871, 5.599213600158691, new Rotation2d())))),
+                    () ->
+                        AllianceFlipUtil.apply(
+                            new Pose2d(11.334761619567871, 5.599213600158691, new Rotation2d())))),
             Map.entry(
                 ZoneId.ZONE_4,
                 new RotateToOrientation(
                     drive,
-                    AllianceFlipUtil.apply(
-                        new Pose2d(11.334761619567871, 2.534952402114868, new Rotation2d()))))),
+                    () ->
+                        AllianceFlipUtil.apply(
+                            new Pose2d(11.334761619567871, 2.534952402114868, new Rotation2d()))))),
         this::getCurrentZone);
   }
 
@@ -225,6 +232,8 @@ public void orchestratorPeriodic() {
             Rotation2d.fromDegrees(0)));
     Logger.recordOutput("Orchestrator/DistanceToHub", shootWhileDrivingResult.distance());
     Logger.recordOutput("Orchestrator/ShooterPosition", shooterLeadCompensator.shooterPose());
+    Logger.recordOutput("Orchestrator/Zone", getCurrentZone());
+    Logger.recordOutput("Orchestrator/Zone", getCurrentZone());
   }
 
   public Command driveToHub() {

From 4f0bd15852f85f0d3c4751b88cf7ab1a8a546787 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 11:15:25 -0500
Subject: [PATCH 048/102] remove robot type override

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 58fb36e35..922aca595 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";

From fa6ffefd92d15b8aeac15365f2cc386b68a3ecce Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 11:39:10 -0500
Subject: [PATCH 049/102] feat: add in a reflectY util improve naming scheme of
 poses for shoot by zone

---
 .../java/frc/lib/utils/AllianceFlipUtil.java  |  7 ++++
 src/main/java/frc/robot/Orchestrator.java     | 38 ++++++-------------
 2 files changed, 18 insertions(+), 27 deletions(-)

diff --git a/src/main/java/frc/lib/utils/AllianceFlipUtil.java b/src/main/java/frc/lib/utils/AllianceFlipUtil.java
index c5b5cf2dc..2efa9ad2e 100644
--- a/src/main/java/frc/lib/utils/AllianceFlipUtil.java
+++ b/src/main/java/frc/lib/utils/AllianceFlipUtil.java
@@ -27,6 +27,13 @@ public static double applyY(double y) {
     return shouldFlip() ? FieldConstants.fieldWidth - y : y;
   }
 
+  public static Pose2d ReflectY(Pose2d pose) {
+    return new Pose2d(
+        pose.getX(),
+        FieldConstants.fieldWidth - pose.getY(),
+        Rotation2d.kZero.minus(pose.getRotation()));
+  }
+
   /** Flips a translation to the correct side of the field based on the current alliance color. */
   public static Translation2d apply(Translation2d translation) {
     return new Translation2d(applyX(translation.getX()), applyY(translation.getY()));
diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 5417a9107..1d69a74d6 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -32,6 +32,11 @@
 import org.littletonrobotics.junction.Logger;
 
 public class Orchestrator {
+  private final Pose2d BBumpClosePose =
+      new Pose2d(4.415811061859131, 5.599213600158691, new Rotation2d());
+  private final Pose2d BBumpFarPose =
+      new Pose2d(11.334761619567871, 5.599213600158691, new Rotation2d());
+
   public enum ZoneId {
     NONE,
     ZONE_1,
@@ -133,43 +138,22 @@ public Command executeCurrentZoneLogic() {
 
     return new SelectCommand<>(
         Map.ofEntries(
-            Map.entry(
-                ZoneId.ZONE_1,
-                new RotateToOrientation(
-                    drive,
-                    () ->
-                        AllianceFlipUtil.apply(
-                            new Pose2d(4.621539115905762, 4.040013313293457, new Rotation2d())))),
+            Map.entry(ZoneId.ZONE_1, aimToHub()),
             Map.entry(
                 ZoneId.ZONE_2,
                 new ConditionalCommand(
+                    new RotateToOrientation(drive, () -> AllianceFlipUtil.apply(BBumpClosePose)),
                     new RotateToOrientation(
                         drive,
-                        () ->
-                            AllianceFlipUtil.apply(
-                                new Pose2d(
-                                    4.415811061859131, 5.599213600158691, new Rotation2d()))),
-                    new RotateToOrientation(
-                        drive,
-                        () ->
-                            AllianceFlipUtil.apply(
-                                new Pose2d(
-                                    4.415811061859131, 2.534952402114868, new Rotation2d()))),
-                    () -> allianceY.getAsDouble() > 4.029185771942139)),
+                        () -> AllianceFlipUtil.apply(AllianceFlipUtil.ReflectY(BBumpClosePose))),
+                    () -> allianceY.getAsDouble() > FieldConstants.fieldWidth / 2)),
             Map.entry(
                 ZoneId.ZONE_3,
-                new RotateToOrientation(
-                    drive,
-                    () ->
-                        AllianceFlipUtil.apply(
-                            new Pose2d(11.334761619567871, 5.599213600158691, new Rotation2d())))),
+                new RotateToOrientation(drive, () -> AllianceFlipUtil.apply(BBumpFarPose))),
             Map.entry(
                 ZoneId.ZONE_4,
                 new RotateToOrientation(
-                    drive,
-                    () ->
-                        AllianceFlipUtil.apply(
-                            new Pose2d(11.334761619567871, 2.534952402114868, new Rotation2d()))))),
+                    drive, () -> AllianceFlipUtil.apply(AllianceFlipUtil.ReflectY(BBumpFarPose))))),
         this::getCurrentZone);
   }
 

From 972881a0f391241ff850cc60e4834fc9f26b9d2e Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 11:40:58 -0500
Subject: [PATCH 050/102] remove current zone duplicate log statement

---
 src/main/java/frc/robot/Orchestrator.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 1d69a74d6..a592b1ec2 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -217,7 +217,6 @@ public void orchestratorPeriodic() {
     Logger.recordOutput("Orchestrator/DistanceToHub", shootWhileDrivingResult.distance());
     Logger.recordOutput("Orchestrator/ShooterPosition", shooterLeadCompensator.shooterPose());
     Logger.recordOutput("Orchestrator/Zone", getCurrentZone());
-    Logger.recordOutput("Orchestrator/Zone", getCurrentZone());
   }
 
   public Command driveToHub() {

From f22758b94fb564087014bbfa593eca30f7edd29a Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 16:19:10 -0400
Subject: [PATCH 051/102] Pass in start poses for pathplanner autos

---
 .../java/frc/robot/commands/auto/Autos.java   | 26 ++++++++++---------
 1 file changed, 14 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 9d3b692dc..3c594a003 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -119,9 +119,9 @@ public Command Bummmmpar() {
   }
 
   // Helper functions that extract common code for pathplanner atuos
-  private Command ppCycle(String path) {
+  private Command ppCycle(String path, Supplier<Pose2d> startPose) {
     return Commands.sequence(
-        Commands.runOnce(() -> drive.setPose(startAside.get())),
+        Commands.runOnce(() -> drive.setPose(startPose.get())),
         Commands.deadline(
                 drive.getAutonomousCommand(path),
                 intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
@@ -135,9 +135,9 @@ private Command ppCycle(String path) {
             orchestrator.feedUp()));
   }
 
-  private Command ppCycleRegression(String path) {
+  private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
     return Commands.sequence(
-        Commands.runOnce(() -> drive.setPose(startBside.get())),
+        Commands.runOnce(() -> drive.setPose(startPose.get())),
         Commands.deadline(
                 drive.getAutonomousCommand(path),
                 intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
@@ -155,34 +155,36 @@ private Command ppCycleRegression(String path) {
             orchestrator.feedUp()));
   }
 
-  private Command pp2CycleRegression(String path1, String path2) {
-    return ppCycleRegression(path1).withTimeout(2.5).andThen(ppCycleRegression(path2));
+  private Command pp2CycleRegression(String path1, String path2, Supplier<Pose2d> startPose) {
+    return ppCycleRegression(path1, startPose)
+        .withTimeout(2.5)
+        .andThen(ppCycleRegression(path2, startPose));
   }
 
   // Pathplanner autos using the helper functions
 
   public Command ppACycleLeft() {
-    return ppCycle("A-Cycle-LeftSweep");
+    return ppCycle("A-Cycle-LeftSweep", startAside);
   }
 
   public Command ppBCycleRight() {
-    return ppCycle("B-Cycle-RightSweep");
+    return ppCycle("B-Cycle-RightSweep", startBside);
   }
 
   public Command ppB2CycleRightRegression() {
-    return pp2CycleRegression("B-Cycle1", "B-Cycle2");
+    return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside);
   }
 
   public Command ppA2CycleRightRegression() {
-    return pp2CycleRegression("A-Cycle1", "A-Cycle2");
+    return pp2CycleRegression("A-Cycle1", "A-Cycle2", startBside);
   }
 
   public Command ppBCycleRightRegression() {
-    return ppCycleRegression("B-Cycle-RightSweep");
+    return ppCycleRegression("B-Cycle-RightSweep", startBside);
   }
 
   public Command ppACycleLeftRegression() {
-    return ppCycleRegression("A-Cycle-LeftSweep");
+    return ppCycleRegression("A-Cycle-LeftSweep", startAside);
   }
 
   // Misc helper functions for manual autos

From 65f3977842ee5adc45114272f64899ff43ce29c0 Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 16:20:46 -0400
Subject: [PATCH 052/102] Pass in start poses for pathplanner autos and fix
 intake calls

---
 src/main/java/frc/robot/commands/auto/Autos.java | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 3c594a003..22fac13b2 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -211,7 +211,7 @@ private Command ccManualAuto(boolean isA) {
     return Commands.sequence(
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(5),
-            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
@@ -225,7 +225,7 @@ private Command ccManualAutoAlt(boolean isA) {
     return Commands.sequence(
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(5),
-            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
@@ -243,7 +243,7 @@ private Command ccManualAutoOverBump(boolean isA) {
     return Commands.sequence(
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(5),
-            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)),
+            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
@@ -259,7 +259,7 @@ private Command ccManualAutoOverBump(boolean isA) {
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(2),
         Commands.deadline(
             new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(3.5),
-            intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS)));
+            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 
   // Manual autos using the helper functions

From 0a4f3edcea6f4f196ca5b8803de9ce344ba9afde Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 16:38:19 -0400
Subject: [PATCH 053/102] Use a record for clarity

---
 .../java/frc/robot/commands/auto/Autos.java   | 34 ++++++++++---------
 1 file changed, 18 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 22fac13b2..ec050324c 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -16,6 +16,8 @@
 import java.util.function.Supplier;
 
 public class Autos {
+  public record AutoPositions(Pose2d intakeSimplePose, Pose2d overBumpAllianceAlt, Pose2d shootFromCorner) { }
+
   private final Drive drive;
   private final Orchestrator orchestrator;
   private final Intake intake;
@@ -195,24 +197,24 @@ private Command shootPullIntake() {
         Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0)));
   }
 
-  List<Pose2d> getPoses(boolean isA) {
+  AutoPositions getPoses(boolean isA) {
     if (isA) {
-      return List.of(
+      return new AutoPositions(
           intakeSimplePoseA.get(), overBumpAllianceAltPoseA.get(), shootFromCornerPoseA.get());
     }
-    return List.of(
+    return new AutoPositions(
         intakeSimplePoseB.get(), overBumpAllianceAltPoseB.get(), shootFromCornerPoseB.get());
   }
 
   // Helper functions that extract common code for manual autos
   private Command ccManualAuto(boolean isA) {
-    List<Pose2d> poses = getPoses(isA);
+    AutoPositions poses = getPoses(isA);
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose)).withTimeout(5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(5),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)).withTimeout(5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
@@ -220,16 +222,16 @@ private Command ccManualAuto(boolean isA) {
   }
 
   private Command ccManualAutoAlt(boolean isA) {
-    List<Pose2d> poses = getPoses(isA);
+    AutoPositions poses = getPoses(isA);
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose)).withTimeout(5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(3.5),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)).withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(2))).withTimeout(2),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.shootFromCorner)).withTimeout(2),
             orchestrator.spinUpShooter(1240)),
         Commands.parallel(
             orchestrator.spinUpShooter(1240),
@@ -238,16 +240,16 @@ private Command ccManualAutoAlt(boolean isA) {
   }
 
   private Command ccManualAutoOverBump(boolean isA) {
-    List<Pose2d> poses = getPoses(isA);
+    AutoPositions poses = getPoses(isA);
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose)).withTimeout(5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(3.5),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)).withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(2))).withTimeout(2),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.shootFromCorner)).withTimeout(2),
             orchestrator.spinUpShooter(1250)),
         Commands.deadline(
             Commands.waitSeconds(5),
@@ -256,9 +258,9 @@ private Command ccManualAutoOverBump(boolean isA) {
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
         rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(1))).withTimeout(2),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)).withTimeout(2),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.get(0))).withTimeout(3.5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose)).withTimeout(3.5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 

From f56aa135ee66b691f7c2d7bda170ab7aa74701f5 Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 16:38:56 -0400
Subject: [PATCH 054/102] Formatting

---
 .../java/frc/robot/commands/auto/Autos.java   | 35 ++++++++++++-------
 1 file changed, 22 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index ec050324c..781f74ebc 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -12,11 +12,11 @@
 import frc.robot.subsystems.intake.IntakeConstants;
 import frc.robot.subsystems.intake.rollers.IntakeRollers;
 import frc.robot.subsystems.shooter.Shooter;
-import java.util.List;
 import java.util.function.Supplier;
 
 public class Autos {
-  public record AutoPositions(Pose2d intakeSimplePose, Pose2d overBumpAllianceAlt, Pose2d shootFromCorner) { }
+  public record AutoPositions(
+      Pose2d intakeSimplePose, Pose2d overBumpAllianceAlt, Pose2d shootFromCorner) {}
 
   private final Drive drive;
   private final Orchestrator orchestrator;
@@ -164,7 +164,6 @@ private Command pp2CycleRegression(String path1, String path2, Supplier<Pose2d>
   }
 
   // Pathplanner autos using the helper functions
-
   public Command ppACycleLeft() {
     return ppCycle("A-Cycle-LeftSweep", startAside);
   }
@@ -212,9 +211,11 @@ private Command ccManualAuto(boolean isA) {
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose)).withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose))
+                .withTimeout(5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)).withTimeout(5),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt))
+            .withTimeout(5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
@@ -226,12 +227,15 @@ private Command ccManualAutoAlt(boolean isA) {
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose)).withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose))
+                .withTimeout(5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)).withTimeout(3.5),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt))
+            .withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.shootFromCorner)).withTimeout(2),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.shootFromCorner))
+                .withTimeout(2),
             orchestrator.spinUpShooter(1240)),
         Commands.parallel(
             orchestrator.spinUpShooter(1240),
@@ -244,12 +248,15 @@ private Command ccManualAutoOverBump(boolean isA) {
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose)).withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose))
+                .withTimeout(5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)).withTimeout(3.5),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt))
+            .withTimeout(3.5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.shootFromCorner)).withTimeout(2),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.shootFromCorner))
+                .withTimeout(2),
             orchestrator.spinUpShooter(1250)),
         Commands.deadline(
             Commands.waitSeconds(5),
@@ -258,9 +265,11 @@ private Command ccManualAutoOverBump(boolean isA) {
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
         rollers.stopIntakeCommand().withTimeout(0.05),
         shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)).withTimeout(2),
+        new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt))
+            .withTimeout(2),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose)).withTimeout(3.5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose))
+                .withTimeout(3.5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 

From e6dc1cd9a162d9d0e45494bb5ab02b65299216a9 Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 16:40:24 -0400
Subject: [PATCH 055/102] Remove unused autos

---
 src/main/java/frc/robot/RobotContainer.java   |  3 --
 .../java/frc/robot/commands/auto/Autos.java   | 35 -------------------
 2 files changed, 38 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c09936056..07a639b6d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -268,8 +268,6 @@ public RobotContainer() {
 
     autoChooser.addDefaultOption("Do Nothing", Commands.none());
 
-    autoChooser.addOption("test path", autos.testPath());
-    autoChooser.addOption("Bummmmpar", autos.Bummmmpar());
     autoChooser.addOption("Manual A-CC", autos.ACCManuelAuto());
     autoChooser.addOption("Manual A-CC-Improved", autos.ACCManuelAutoAlt());
     autoChooser.addOption("Manual A-CC-Over-Bump", autos.ACCManuelAutoOverBump());
@@ -278,7 +276,6 @@ public RobotContainer() {
     autoChooser.addOption("Manual B-CC-Improved", autos.BCCManuelAutoAlt());
     autoChooser.addOption("Manual B-CC-Over-Bump", autos.BCCManuelAutoOverBump());
     autoChooser.addOption("test path 2", drive.getAutonomousCommand("test path 2"));
-    autoChooser.addOption("CL auto", autos.CenterA());
     autoChooser.addOption("8 Ball Auto", autos.EightBalls());
 
     autoChooser.addOption(
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 781f74ebc..9d617a5a2 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -85,41 +85,6 @@ public Autos(
   private static final Supplier<Pose2d> startBside =
       () -> AllianceFlipUtil.apply(new Pose2d(3.645, 2.516, new Rotation2d(0.000)));
 
-  public Command CenterA() {
-    return Commands.sequence(
-        Commands.runOnce(() -> drive.setPose(AllianceFlipUtil.apply(CenterA.get()))),
-        drive.getAutonomousCommand("CA-out-intake"),
-        new StraightDriveToPose(climb.get(), drive).withTimeout(2.0));
-  }
-
-  public Command testPath() {
-    return Commands.sequence(
-        Commands.runOnce(
-            () ->
-                drive.setPose(
-                    AllianceFlipUtil.apply(
-                        new Pose2d(3.213, 5.600, new Rotation2d(Units.degreesToRadians(0)))))),
-        drive.getAutonomousCommand("test path"));
-  }
-
-  public Command Bummmmpar() {
-    return Commands.sequence(
-        Commands.runOnce(
-            () ->
-                drive.setPose(
-                    AllianceFlipUtil.apply(
-                        new Pose2d(3.213, 5.600, new Rotation2d(Units.degreesToRadians(0)))))),
-        new DriveToPose(
-            drive,
-            () -> AllianceFlipUtil.apply(new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(0)))),
-        new DriveToPose(
-            drive,
-            () -> AllianceFlipUtil.apply(new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(0)))),
-        new DriveToPose(
-            drive,
-            () -> AllianceFlipUtil.apply(new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(0)))));
-  }
-
   // Helper functions that extract common code for pathplanner atuos
   private Command ppCycle(String path, Supplier<Pose2d> startPose) {
     return Commands.sequence(

From 20f6f60169868d04d9350aa61d642f07d3aa41e0 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 11:46:09 -0500
Subject: [PATCH 056/102] rename zone based shooting to zoneBasedAim

---
 src/main/java/frc/robot/Orchestrator.java | 11 +----------
 1 file changed, 1 insertion(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index a592b1ec2..f1e747c7e 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -133,7 +133,7 @@ public ZoneId getCurrentZone() {
     return ZoneId.NONE;
   }
 
-  public Command executeCurrentZoneLogic() {
+  public Command zoneBasedAim() {
     DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY());
 
     return new SelectCommand<>(
@@ -157,15 +157,6 @@ public Command executeCurrentZoneLogic() {
         this::getCurrentZone);
   }
 
-  public Command zoneBasedShoot() {
-    return Commands.deadline(
-            executeCurrentZoneLogic(),
-            feedUp(),
-            Commands.run(
-                () -> Logger.recordOutput("Orchestrator/CurrentZone", getCurrentZone().name())))
-        .withName("zoneBasedShoot");
-  }
-
   public Pose2d getShootWhileDrivingResultPose() {
     var shootWhileDrivingResult =
         shooterLeadCompensator.shootWhileDriving(

From 30e27233e9b40276243d2d0584ca832bb75a8fe4 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 11:50:20 -0500
Subject: [PATCH 057/102] revert RobotContainer

---
 src/main/java/frc/robot/RobotContainer.java | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1e02f3685..5b7c1604b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -360,8 +360,7 @@ private void configureButtonBindings() {
         .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
 
     //     VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
-    //    driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
-    driverController.leftTrigger(0.2).toggleOnTrue(orchestrator.zoneBasedShoot());
+    driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
 
     driverController

From e44f6e9ce5830e0ab35af9a4ea1406026f795166 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 11:51:35 -0500
Subject: [PATCH 058/102] change ReflectY to lowercase

---
 src/main/java/frc/lib/utils/AllianceFlipUtil.java | 2 +-
 src/main/java/frc/robot/Orchestrator.java         | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/lib/utils/AllianceFlipUtil.java b/src/main/java/frc/lib/utils/AllianceFlipUtil.java
index 2efa9ad2e..c5d1063c0 100644
--- a/src/main/java/frc/lib/utils/AllianceFlipUtil.java
+++ b/src/main/java/frc/lib/utils/AllianceFlipUtil.java
@@ -27,7 +27,7 @@ public static double applyY(double y) {
     return shouldFlip() ? FieldConstants.fieldWidth - y : y;
   }
 
-  public static Pose2d ReflectY(Pose2d pose) {
+  public static Pose2d reflectY(Pose2d pose) {
     return new Pose2d(
         pose.getX(),
         FieldConstants.fieldWidth - pose.getY(),
diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index f1e747c7e..ccc081f6f 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -145,7 +145,7 @@ public Command zoneBasedAim() {
                     new RotateToOrientation(drive, () -> AllianceFlipUtil.apply(BBumpClosePose)),
                     new RotateToOrientation(
                         drive,
-                        () -> AllianceFlipUtil.apply(AllianceFlipUtil.ReflectY(BBumpClosePose))),
+                        () -> AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpClosePose))),
                     () -> allianceY.getAsDouble() > FieldConstants.fieldWidth / 2)),
             Map.entry(
                 ZoneId.ZONE_3,
@@ -153,7 +153,7 @@ public Command zoneBasedAim() {
             Map.entry(
                 ZoneId.ZONE_4,
                 new RotateToOrientation(
-                    drive, () -> AllianceFlipUtil.apply(AllianceFlipUtil.ReflectY(BBumpFarPose))))),
+                    drive, () -> AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpFarPose))))),
         this::getCurrentZone);
   }
 

From 863ae6a68b94243e3bdc3d920ecc91de72c98670 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 11:53:51 -0500
Subject: [PATCH 059/102] remove duplicate current zone

---
 src/main/java/frc/robot/Orchestrator.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index ccc081f6f..8e7d388e5 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -207,7 +207,6 @@ public void orchestratorPeriodic() {
             Rotation2d.fromDegrees(0)));
     Logger.recordOutput("Orchestrator/DistanceToHub", shootWhileDrivingResult.distance());
     Logger.recordOutput("Orchestrator/ShooterPosition", shooterLeadCompensator.shooterPose());
-    Logger.recordOutput("Orchestrator/Zone", getCurrentZone());
   }
 
   public Command driveToHub() {

From 97dbcc666d605986b3b4dfdc8db4644e54d47e36 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sat, 11 Apr 2026 14:46:53 -0400
Subject: [PATCH 060/102] cycle2 autos command and starting pose fixes

---
 .../deploy/pathplanner/paths/A-Cycle1.path    | 26 +++++++++----------
 .../deploy/pathplanner/paths/A-Cycle2.path    | 10 +++----
 .../deploy/pathplanner/paths/B-Cycle1.path    | 18 ++++++-------
 .../deploy/pathplanner/paths/B-Cycle2.path    |  2 +-
 src/main/java/frc/robot/Constants.java        |  2 +-
 src/main/java/frc/robot/RobotContainer.java   |  5 ++++
 .../java/frc/robot/commands/auto/Autos.java   | 23 +++++++++++-----
 7 files changed, 50 insertions(+), 36 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index cc35c91cd..9d22515a8 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 4.294776386404293,
-        "y": 7.530574239713776
+        "x": 4.295,
+        "y": 7.531
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.22064231261789,
-        "y": 7.586659838431375
+        "x": 5.220865926213596,
+        "y": 7.587085598717599
       },
       "isLocked": false,
       "linkedName": null
@@ -64,12 +64,12 @@
     },
     {
       "anchor": {
-        "x": 3.256350626118068,
-        "y": 5.664652951699464
+        "x": 3.256,
+        "y": 5.665
       },
       "prevControl": {
-        "x": 4.3759033989266545,
-        "y": 5.729554561717354
+        "x": 4.375552524919555,
+        "y": 5.729905885962339
       },
       "nextControl": null,
       "isLocked": false,
@@ -155,11 +155,11 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4,
-    "maxAcceleration": 6,
-    "maxAngularVelocity": 540,
-    "maxAngularAcceleration": 720,
-    "nominalVoltage": 12,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 6.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index c7074f4f9..976a35738 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -172,11 +172,11 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4,
-    "maxAcceleration": 6,
-    "maxAngularVelocity": 540,
-    "maxAngularAcceleration": 720,
-    "nominalVoltage": 12,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 6.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index 7c38315c8..14317bd18 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 4.294776386404293,
-        "y": 0.5384257602862248
+        "x": 4.295,
+        "y": 0.538
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.22064231261789,
-        "y": 0.48234016156862536
+        "x": 5.220865926213596,
+        "y": 0.4819144012824006
       },
       "isLocked": false,
       "linkedName": null
@@ -64,12 +64,12 @@
     },
     {
       "anchor": {
-        "x": 3.1752236135957066,
-        "y": 2.4043470483005365
+        "x": 3.175,
+        "y": 2.404
       },
       "prevControl": {
-        "x": 4.2947763864042905,
-        "y": 2.339445438282647
+        "x": 4.294552524919553,
+        "y": 2.3390941140376613
       },
       "nextControl": null,
       "isLocked": false,
@@ -164,7 +164,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 51.952957468173956
+    "rotation": 51.95295746817396
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path
index 4eecf8061..5ddf30eec 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path
@@ -187,7 +187,7 @@
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 51.952957468173956
+    "rotation": 51.95295746817396
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 922aca595..58fb36e35 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b9fb8ac3c..4421dc985 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -302,6 +302,11 @@ public RobotContainer() {
         "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
     autoChooser.addOption(
         "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+    // pp
+
+    autoChooser.addOption("A-side", autos.ppA2CycleRightRegression());
+    autoChooser.addOption("B-side", autos.ppB2CycleRightRegression());
     // Configure the button bindings
     configureButtonBindings();
   }
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 9d617a5a2..27bec0c0e 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -81,9 +81,17 @@ public Autos(
   // pp
 
   private static final Supplier<Pose2d> startAside =
-      () -> AllianceFlipUtil.apply(new Pose2d(3.645, 5.520, new Rotation2d(0.000)));
+      () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000)));
   private static final Supplier<Pose2d> startBside =
-      () -> AllianceFlipUtil.apply(new Pose2d(3.645, 2.516, new Rotation2d(0.000)));
+      () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000)));
+  private static final Supplier<Pose2d> connectBside =
+      () ->
+          AllianceFlipUtil.apply(
+              new Pose2d(3.175, 2.404, new Rotation2d(Units.degreesToRadians(-118.29 + 180))));
+  private static final Supplier<Pose2d> connectAside =
+      () ->
+          AllianceFlipUtil.apply(
+              new Pose2d(3.256, 5.665, new Rotation2d(Units.degreesToRadians(115.83 + 180))));
 
   // Helper functions that extract common code for pathplanner atuos
   private Command ppCycle(String path, Supplier<Pose2d> startPose) {
@@ -122,10 +130,11 @@ private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
             orchestrator.feedUp()));
   }
 
-  private Command pp2CycleRegression(String path1, String path2, Supplier<Pose2d> startPose) {
+  private Command pp2CycleRegression(
+      String path1, String path2, Supplier<Pose2d> startPose, Supplier<Pose2d> startPose2) {
     return ppCycleRegression(path1, startPose)
-        .withTimeout(2.5)
-        .andThen(ppCycleRegression(path2, startPose));
+        .withTimeout(10.5)
+        .andThen(ppCycleRegression(path2, startPose2));
   }
 
   // Pathplanner autos using the helper functions
@@ -138,11 +147,11 @@ public Command ppBCycleRight() {
   }
 
   public Command ppB2CycleRightRegression() {
-    return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside);
+    return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside, connectBside);
   }
 
   public Command ppA2CycleRightRegression() {
-    return pp2CycleRegression("A-Cycle1", "A-Cycle2", startBside);
+    return pp2CycleRegression("A-Cycle1", "A-Cycle2", startAside, connectAside);
   }
 
   public Command ppBCycleRightRegression() {

From 168f468c80f3ade203230b08494da9af5e038000 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sat, 11 Apr 2026 14:54:38 -0400
Subject: [PATCH 061/102] robot cosntant

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 58fb36e35..922aca595 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,7 @@
 
 public class Constants {
   // Change this temporarily to override the RobotType, e.g. RobotType.ROBOT_SIMBOT
-  private static final RobotType ROBOT_TYPE_OVERRIDE = RobotType.ROBOT_SIMBOT;
+  private static final RobotType ROBOT_TYPE_OVERRIDE = null;
   public static final boolean tuningMode = false;
 
   private static final String ROBOT_FILENAME = "/home/lvuser/robot";

From 66b37e639cabd6410b962736da1b28eac7988933 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 14:48:58 -0500
Subject: [PATCH 062/102] fix intake io and shooter schematic

---
 src/main/java/frc/robot/Schematic.java              |  8 ++++----
 .../robot/subsystems/intake/IntakeConstants.java    |  6 +++---
 .../subsystems/intake/extend/IntakeExtend.java      |  4 ++++
 .../intake/extend/IntakeExtendIOSparkMax.java       | 13 ++++++-------
 4 files changed, 17 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java
index 4e67e29d1..983a35d83 100644
--- a/src/main/java/frc/robot/Schematic.java
+++ b/src/main/java/frc/robot/Schematic.java
@@ -101,10 +101,10 @@ public class Schematic {
         backLeftAbsoluteEncoderCanId = 21;
 
         // Shooter (CAN Ids)
-        shooterTopLeftMotorCanId = 11;
-        shooterTopRightMotorCanId = 12;
-        shooterBottomLeftMotorCanId = 13;
-        shooterBottomRightMotorCanId = 14;
+        shooterTopLeftMotorCanId = 33;
+        shooterTopRightMotorCanId = 35;
+        shooterBottomLeftMotorCanId = 34;
+        shooterBottomRightMotorCanId = 36;
 
         // Hopper (CAN Ids)
         magicCarpetCanId = 15;
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index b30e2f1d0..2cdc3288d 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -18,12 +18,12 @@ public class IntakeConstants {
 
   public static final int EXTEND_LIMIT_ID = 1;
 
-  public static final double PID_P = 0.023; // arbritrary values
+  public static final double PID_P = 0.0023; // arbritrary values
   public static final double PID_I = 0.000004;
   public static final double PID_D = 0.00001;
 
-  public static final double EXTEND_POS = -33; // TODO: change the actual values
-  public static final double FUNNEL_POS = -16.5;
+  public static final double EXTEND_POS = -13; // TODO: change the actual values
+  public static final double FUNNEL_POS = -7;
 
   // shake around the funnel pos by this much
   public static final double SHAKE_POS_OFFSET = 1;
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index d693d9cf0..2b0621a3b 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -70,6 +70,10 @@ public DoubleSupplier getAngle() {
     return () -> inputs.getExtendPos;
   }
 
+  public Command setPose(double pose) {
+    return Commands.runOnce(() -> io.resetExtendEncoder(pose), this);
+  }
+
   private boolean isStallingExtend() {
     // For our volts, we are not getting the right velocity
     return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts < -0.01;
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java
index 912229481..ed688a961 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java
@@ -17,7 +17,7 @@
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.AlternateEncoderConfig;
+import com.revrobotics.spark.config.EncoderConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -33,10 +33,9 @@ public class IntakeExtendIOSparkMax implements IntakeExtendIO {
 
   public IntakeExtendIOSparkMax() {
     extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless);
-    AlternateEncoderConfig extendEnc = new AlternateEncoderConfig();
+    EncoderConfig extendEnc = new EncoderConfig();
     extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION);
     extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION);
-    extendEnc.inverted(true);
     var extendConfig = new SparkMaxConfig();
     extendConfig
         .inverted(false)
@@ -45,11 +44,11 @@ public IntakeExtendIOSparkMax() {
         .smartCurrentLimit((int) Math.round(INTAKE_EXTEND_MOTOR_CURRENT_LIMIT))
         .closedLoop
         .pid(PID_P, PID_I, PID_D)
-        .feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder);
-    extendConfig.alternateEncoder.apply(extendEnc);
+        .feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+    extendConfig.encoder.apply(extendEnc);
 
     pidController = extendMotor.getClosedLoopController();
-    extendMotorEncoder = extendMotor.getAlternateEncoder();
+    extendMotorEncoder = extendMotor.getEncoder();
 
     extendMotor.configure(
         extendConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
@@ -62,7 +61,7 @@ public void updateInputs(IntakeExtendIOInputs inputs) {
     inputs.extendVelocity = extendMotorEncoder.getVelocity();
     inputs.extendVolts = extendMotor.getAppliedOutput();
     inputs.extendAmps = extendMotor.getOutputCurrent();
-    inputs.isCollapsed = collapsedLimitSwitch.get();
+    inputs.isCollapsed = false;
     inputs.getExtendPos = extendMotorEncoder.getPosition();
     inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint() && usingPID;
     inputs.hasSetpoint = usingPID;

From e45a3a2149a12c0801584fa8885d842645dcd09f Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 16:04:22 -0500
Subject: [PATCH 063/102] magic carpet and indexer implementation for v2 -
 tested on robot ready for merge

---
 src/main/java/frc/robot/RobotContainer.java   | 10 +++------
 .../frc/robot/subsystems/indexer/Indexer.java |  2 ++
 .../subsystems/intake/IntakeConstants.java    |  2 +-
 .../subsystems/magicCarpet/MagicCarpet.java   | 21 ++++++++++++++-----
 .../magicCarpet/MagicCarpetConstants.java     |  2 +-
 5 files changed, 23 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b9fb8ac3c..0b0b9d897 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -195,7 +195,6 @@ public RobotContainer() {
         "endIntake",
         Commands.parallel(
                 intakeRollers.stopIntakeCommand().withTimeout(0.05),
-                magicCarpet.stop().withTimeout(0.05),
                 indexer.stop().withTimeout(0.05))
             .withTimeout(0.05));
     NamedCommands.registerCommand(
@@ -215,7 +214,6 @@ public RobotContainer() {
                             Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM))
                         .withTimeout(0.8),
                     intakeRollers.stopIntakeCommand(),
-                    magicCarpet.stop(),
                     indexer.stop())
                 .withTimeout(0.8),
             Commands.deadline(
@@ -223,9 +221,7 @@ public RobotContainer() {
                 shooter.setTargetVelocityRadiansRepeatedly(
                     Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM)),
                 orchestrator.feedUp()),
-            Commands.parallel(
-                    magicCarpet.stop().withTimeout(0.05), indexer.stop().withTimeout(0.05))
-                .withTimeout(0.05)));
+            Commands.parallel(indexer.stop().withTimeout(0.05)).withTimeout(0.05)));
     AutoBuilder.configure(
         drive::getPose,
         drive::setPose,
@@ -313,7 +309,6 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
-    magicCarpet.setDefaultCommand(magicCarpet.stop());
     indexer.setDefaultCommand(indexer.stop());
     shooter.setDefaultCommand(shooter.stop());
     //    shooter.setDefaultCommand(shooter.stop());
@@ -358,7 +353,8 @@ private void configureButtonBindings() {
 
     //     VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
     driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
-    driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
+    //    driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
+    driverController.rightTrigger(0.1).toggleOnTrue(indexer.run());
 
     driverController
         .a()
diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
index 2c3840830..6f43a6785 100644
--- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotState;
 import org.littletonrobotics.junction.Logger;
 
 public class Indexer extends SubsystemBase {
@@ -19,6 +20,7 @@ public Indexer(IndexerIO io) {
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Index", inputs);
+    RobotState.getInstance().indexerRunning = inputs.feedUpVolts > 0;
   }
 
   private void setPercent(double indexPercent, double feedUpPercent) {
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 2cdc3288d..ee3abab23 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -18,7 +18,7 @@ public class IntakeConstants {
 
   public static final int EXTEND_LIMIT_ID = 1;
 
-  public static final double PID_P = 0.0023; // arbritrary values
+  public static final double PID_P = 0.023; // arbritrary values
   public static final double PID_I = 0.000004;
   public static final double PID_D = 0.00001;
 
diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java
index aa507f61b..c36fdc122 100644
--- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java
+++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java
@@ -3,29 +3,40 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotState;
 import org.littletonrobotics.junction.Logger;
 
 public class MagicCarpet extends SubsystemBase {
   private final MagicCarpetIO io;
   private final MagicCarpetIOInputsAutoLogged inputs = new MagicCarpetIOInputsAutoLogged();
+  public boolean manualRun;
 
   public MagicCarpet(MagicCarpetIO io) {
     this.io = io;
+    this.manualRun = false;
   }
 
   public Command run() {
-    return Commands.run(() -> io.setSpeed(MagicCarpetConstants.BELT_SPEED), this)
-        .withName("start")
-        .finallyDo(this::stop);
+    return setManualControl(true)
+        .andThen(Commands.run(() -> io.setSpeed(MagicCarpetConstants.BELT_SPEED), this))
+        .finallyDo(() -> setManualControl(false).schedule())
+        .withName("start");
   }
 
-  public Command stop() {
-    return Commands.run(() -> io.setSpeed(0.0), this).withName("stop");
+  public Command setManualControl(boolean manual) {
+    return Commands.runOnce(() -> this.manualRun = manual, this);
   }
 
   @Override
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("magicCarpet", inputs);
+    if (!manualRun) {
+      if (RobotState.getInstance().indexerRunning) {
+        io.setSpeed(MagicCarpetConstants.BELT_SPEED);
+      } else {
+        io.setSpeed(0.0);
+      }
+    }
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
index 8980bcc33..dff198558 100644
--- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
+++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
@@ -2,7 +2,7 @@
 
 public final class MagicCarpetConstants {
 
-  public static final boolean MOTOR_INVERTED = true;
+  public static final boolean MOTOR_INVERTED = false;
 
   public static final int CURRENT_LIMIT = 30;
   public static final double BELT_SPEED = 0.8; // TODO: bump this value up if needed

From 9294917b0b7173b4640843a0d6a85a265cc3e9f4 Mon Sep 17 00:00:00 2001
From: Ronit Sharma <110395974+Rshar2027@users.noreply.github.com>
Date: Sat, 11 Apr 2026 17:08:11 -0400
Subject: [PATCH 064/102] Drivetrain fixes (#458)

* change turnmotorreduction

* set all rotations to zero

* drive rotations

* formatting

* switched shooter can ids

* drivetrain turn offsets tuned - tested on robot ready for merge

* fix intake io and shooter schematic

---------

Co-authored-by: Suhaas Goluguri <suhaas.goluguri@gmail.com>
Co-authored-by: Elliot Moore <eqm.emoore@hotmail.com>
---
 src/main/java/frc/robot/Schematic.java        | 12 ++++-----
 .../subsystems/drive/DriveConstants.java      | 25 ++++++++++---------
 2 files changed, 19 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java
index 983a35d83..0707243cf 100644
--- a/src/main/java/frc/robot/Schematic.java
+++ b/src/main/java/frc/robot/Schematic.java
@@ -85,19 +85,19 @@ public class Schematic {
         pigeonCanId = 17;
 
         // Drive (CAN Ids)
-        frontLeftDriveCanId = 1;
-        frontLeftTurnCanId = 2;
+        frontLeftDriveCanId = 5;
+        frontLeftTurnCanId = 6;
         frontRightDriveCanId = 3;
         frontRightTurnCanId = 4;
 
-        backRightDriveCanId = 5;
-        backRightTurnCanId = 6;
+        backRightDriveCanId = 1;
+        backRightTurnCanId = 2;
         backLeftDriveCanId = 7;
         backLeftTurnCanId = 8;
 
-        frontLeftAbsoluteEncoderCanId = 18;
+        frontLeftAbsoluteEncoderCanId = 20;
         frontRightAbsoluteEncoderCanId = 19;
-        backRightAbsoluteEncoderCanId = 20;
+        backRightAbsoluteEncoderCanId = 18;
         backLeftAbsoluteEncoderCanId = 21;
 
         // Shooter (CAN Ids)
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index 83593e168..ce75e28ba 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems.drive;
 
 import static frc.robot.Schematic.*;
+import static java.lang.Math.PI;
 
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType;
@@ -35,10 +36,10 @@ public class DriveConstants {
   static {
     switch (Constants.getRobot()) {
       case ROBOT_2026_COMP -> {
-        frontLeftZeroRotation = Rotation2d.fromDegrees(-2.46);
-        frontRightZeroRotation = Rotation2d.fromDegrees(-10.63);
-        backLeftZeroRotation = Rotation2d.fromDegrees(17.11 + 180);
-        backRightZeroRotation = Rotation2d.fromDegrees(-45.27 + 180);
+        frontLeftZeroRotation = Rotation2d.fromRadians(0.126 + PI);
+        frontRightZeroRotation = Rotation2d.fromRadians(0.759 + 0.03);
+        backLeftZeroRotation = Rotation2d.fromRadians(-1.630);
+        backRightZeroRotation = Rotation2d.fromRadians(-2.072);
       }
       case ROBOT_2025_COMP -> {
         frontLeftZeroRotation = Rotation2d.fromDegrees(88.42 - 45);
@@ -66,9 +67,9 @@ public class DriveConstants {
 
   // Drive encoder configuration
   public static final double driveEncoderPositionFactor =
-      2 * Math.PI / driveMotorReduction; // Rotor Rotations -> Wheel Radians
+      2 * PI / driveMotorReduction; // Rotor Rotations -> Wheel Radians
   public static final double driveEncoderVelocityFactor =
-      (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec
+      (2 * PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec
 
   // Drive PID configuration
   public static final double driveKp = 2.525;
@@ -84,26 +85,26 @@ public class DriveConstants {
   // Turn motor configuration
   public static final boolean turnInverted = false;
   public static final int turnMotorCurrentLimit = 10;
-  public static final double turnMotorReduction = 12.8;
+  public static final double turnMotorReduction = 26;
   public static final DCMotor turnGearbox = DCMotor.getNEO(1);
 
   // Turn encoder configuration
   public static final boolean turnEncoderInverted = false;
   public static final double turnEncoderPositionFactor =
-      2 * Math.PI / turnMotorReduction; // Rotations -> Radians
+      2 * PI / turnMotorReduction; // Rotations -> Radians
   public static final double turnEncoderVelocityFactor =
-      (2 * Math.PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec
+      (2 * PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec
 
   // Turn PID configuration
   public static final double turnKp = 3.5;
   public static final double turnKd = 0.0;
   public static final double turnSimP = 9.0;
   public static final double turnSimD = 0.0;
-  public static final double turnPIDMinInput = -Math.PI; // Radians
-  public static final double turnPIDMaxInput = Math.PI; // Radians
+  public static final double turnPIDMinInput = -PI; // Radians
+  public static final double turnPIDMaxInput = PI; // Radians
 
   // PathPlanner configuration
-  public static final double robotMassKg = 74.088;
+  public static final double robotMassKg = 51.00;
   public static final double robotMOI = 6.883;
   public static final double wheelCOF = 1.2;
   public static final RobotConfig ppConfig =

From c2da1a6f1201fbbadb5345c4fe9012e07990ba8a Mon Sep 17 00:00:00 2001
From: Ekansh Jain <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 17:13:49 -0400
Subject: [PATCH 065/102] Opus Clean (#454)

* Add initial docs

* Finish auto docstrings

* Add class docstrings

* Fix paths

* Some edits

* Greatly improve organization for autos

* Switch shooter to units library

* Add a few comments

* Use the Units library!

* Formatting

* Fix wait time

* Add back supplier

* Fixes
---
 src/main/java/frc/robot/Orchestrator.java     |  44 +--
 src/main/java/frc/robot/RobotContainer.java   |  28 +-
 .../java/frc/robot/commands/auto/Autos.java   | 329 +++++++++++-------
 .../java/frc/robot/commands/auto/README.md    |  10 +
 .../frc/robot/subsystems/climber/Climber.java |  64 ----
 .../subsystems/climber/ClimberConstants.java  |  23 --
 .../robot/subsystems/climber/ClimberIO.java   |  25 --
 .../subsystems/climber/ClimberIOPhysical.java |  98 ------
 .../subsystems/climber/ClimberIOSim.java      |   3 -
 .../subsystems/magicCarpet/MagicCarpet.java   |  15 +
 .../frc/robot/subsystems/shooter/Shooter.java | 136 +++++---
 .../subsystems/shooter/ShooterIOSparkMax.java |   4 -
 .../robot/util/ShooterLeadCompensator.java    |   8 +-
 13 files changed, 356 insertions(+), 431 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/auto/README.md
 delete mode 100644 src/main/java/frc/robot/subsystems/climber/Climber.java
 delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberConstants.java
 delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIO.java
 delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIOPhysical.java
 delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 8e7d388e5..5c9ed0a49 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -1,5 +1,9 @@
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Minute;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Rotations;
 import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM;
 
 import edu.wpi.first.math.filter.LinearFilter;
@@ -7,6 +11,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -29,6 +34,7 @@
 import frc.robot.util.Zone.Tuple2d;
 import java.util.Map;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
 public class Orchestrator {
@@ -167,19 +173,20 @@ public Pose2d getShootWhileDrivingResultPose() {
         Rotation2d.fromDegrees(0));
   }
 
-  public DoubleSupplier getShootWhileDrivingResultDistance() {
+  public Supplier<Distance> getShootWhileDrivingResultDistance() {
     return () -> {
       var shootWhileDrivingResult =
           shooterLeadCompensator.shootWhileDriving(
               AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()));
-      return shootWhileDrivingResult.distance();
+      return Meters.of(shootWhileDrivingResult.distance());
     };
   }
 
-  public DoubleSupplier getHubDistance() {
+  public Supplier<Distance> getHubDistance() {
     return () ->
-        AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d())
-            .getDistance(drive.getPose().getTranslation());
+        Meters.of(
+            AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d())
+                .getDistance(drive.getPose().getTranslation()));
   }
 
   private Rotation2d filteredHubAngle(Rotation2d raw) {
@@ -229,37 +236,30 @@ public Command feedUp() {
     return Commands.repeatingSequence(
             Commands.parallel(magicCarpet.run(), indexer.run())
                 .until(() -> !RobotState.getInstance().shooterAtSpeed))
-        .onlyIf(() -> shooter.getSetpoint() > 0)
+        .onlyIf(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0)))
         .onlyIf(() -> RobotState.getInstance().shooterAtSpeed)
-        .onlyWhile(() -> shooter.getSetpoint() > 0)
+        .onlyWhile(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0)))
         .withName("feedUp");
   }
 
   public Command spinUpShooterTest() {
     SmartDashboard.putNumber("Shooter/TestRPM", CLOSE_HUB_SHOOTER_RPM);
     return shooter
-        .setTargetVelocityRadians(
-            () ->
-                Units.rotationsPerMinuteToRadiansPerSecond(
-                    SmartDashboard.getNumber("Shooter/TestRPM", 1000.0)))
+        .setTargetVelocity(
+            () -> Rotations.per(Minute).of(SmartDashboard.getNumber("Shooter/TestRPM", 1000.0)))
         .withName("spinUpShooterTest");
   }
 
-  public Command spinUpShooterDistance(DoubleSupplier targetDistance) {
-    return shooter.setTargetVelocityRadians(
-        () ->
-            Units.rotationsPerMinuteToRadiansPerSecond(
-                shooter.calculateSetpoint(targetDistance).getAsDouble()));
+  public Command spinUpShooterDistance(Supplier<Distance> targetDistance) {
+    return shooter.setTargetVelocity(shooter.calculateSetpoint(targetDistance));
   }
 
   public Command spinUpShooterHub() {
-    return shooter.setTargetVelocityRadians(
-        Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM));
+    return shooter.setTargetVelocity(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM));
   }
 
   public Command spinUpShooter(double velocityRPM) {
-    return shooter.setTargetVelocityRadians(
-        Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM));
+    return shooter.setTargetVelocity(Rotations.per(Minute).of(velocityRPM));
   }
 
   public Command driveShootAtAngle() {
@@ -280,8 +280,8 @@ public Command driveShootAtAngle() {
   }
 
   public Command spinUpShooterWhileDriving() {
-    return shooter.setTargetVelocityRadians(
-        () -> shooter.calculateSetpoint(this.getShootWhileDrivingResultDistance()).getAsDouble());
+    return shooter.setTargetVelocity(
+        shooter.calculateSetpoint(this.getShootWhileDrivingResultDistance()));
   }
 
   public Command aimToHub() {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 0af612f94..151a6d629 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.*;
 import static frc.robot.subsystems.drive.DriveConstants.ppConfig;
 import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM;
 import static frc.robot.subsystems.vision.VisionConstants.*;
@@ -15,7 +16,6 @@
 import com.pathplanner.lib.pathfinding.Pathfinding;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import edu.wpi.first.math.geometry.*;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -32,8 +32,6 @@
 import frc.robot.commands.auto.DriveToPose;
 import frc.robot.commands.drive.DriveCommands;
 import frc.robot.commands.drive.DriveWithDpad;
-import frc.robot.subsystems.climber.Climber;
-import frc.robot.subsystems.climber.ClimberIO;
 import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.indexer.IndexerIO;
@@ -75,7 +73,6 @@ public class RobotContainer {
   private Indexer indexer;
   private final Orchestrator orchestrator;
   private Shooter shooter;
-  private Climber climber;
   private Intake intake;
   private IntakeRollers intakeRollers;
   private IntakeExtend intakeExtend;
@@ -181,9 +178,6 @@ public RobotContainer() {
     if (indexer == null) {
       indexer = new Indexer(new IndexerIO() {});
     }
-    if (climber == null) {
-      climber = new Climber(new ClimberIO() {});
-    }
 
     orchestrator =
         new Orchestrator(
@@ -199,8 +193,7 @@ public RobotContainer() {
             .withTimeout(0.05));
     NamedCommands.registerCommand(
         "spinUp",
-        shooter.setTargetVelocityRadiansRepeatedly(
-            Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM)));
+        shooter.setTargetVelocityRepeatedly(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)));
     NamedCommands.registerCommand("feedShooter", orchestrator.feedUp());
     NamedCommands.registerCommand("bringInIntake", intake.extendToAngleAndIntake(0));
     NamedCommands.registerCommand("driveToHub", orchestrator.driveToHub());
@@ -210,16 +203,16 @@ public RobotContainer() {
         Commands.sequence(
             Commands.parallel(
                     shooter
-                        .setTargetVelocityRadiansRepeatedly(
-                            Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM))
+                        .setTargetVelocityRepeatedly(
+                            Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM))
                         .withTimeout(0.8),
                     intakeRollers.stopIntakeCommand(),
                     indexer.stop())
                 .withTimeout(0.8),
             Commands.deadline(
                 Commands.waitSeconds(3.2),
-                shooter.setTargetVelocityRadiansRepeatedly(
-                    Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM)),
+                shooter.setTargetVelocityRepeatedly(
+                    Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)),
                 orchestrator.feedUp()),
             Commands.parallel(indexer.stop().withTimeout(0.05)).withTimeout(0.05)));
     AutoBuilder.configure(
@@ -264,13 +257,12 @@ public RobotContainer() {
 
     autoChooser.addDefaultOption("Do Nothing", Commands.none());
 
-    autoChooser.addOption("Manual A-CC", autos.ACCManuelAuto());
+    autoChooser.addOption("Manual A-CC", autos.ACCManualAuto());
     autoChooser.addOption("Manual A-CC-Improved", autos.ACCManuelAutoAlt());
-    autoChooser.addOption("Manual A-CC-Over-Bump", autos.ACCManuelAutoOverBump());
-    autoChooser.addOption("Manual ADepot", autos.ADepot());
-    autoChooser.addOption("Manual B-CC", autos.BCCManuelAuto());
+    autoChooser.addOption("Manual A-CC-Over-Bump", autos.ACCManualAutoOverBump());
+    autoChooser.addOption("Manual B-CC", autos.BCCManualAuto());
     autoChooser.addOption("Manual B-CC-Improved", autos.BCCManuelAutoAlt());
-    autoChooser.addOption("Manual B-CC-Over-Bump", autos.BCCManuelAutoOverBump());
+    autoChooser.addOption("Manual B-CC-Over-Bump", autos.BCCManualAutoOverBump());
     autoChooser.addOption("test path 2", drive.getAutonomousCommand("test path 2"));
     autoChooser.addOption("8 Ball Auto", autos.EightBalls());
 
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 27bec0c0e..c145ea0e5 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -14,16 +14,65 @@
 import frc.robot.subsystems.shooter.Shooter;
 import java.util.function.Supplier;
 
+/** Contains all autos */
 public class Autos {
-  public record AutoPositions(
-      Pose2d intakeSimplePose, Pose2d overBumpAllianceAlt, Pose2d shootFromCorner) {}
 
+  /**
+   * A helper class that tracks poses needed for manual autos. It simplifies switching between left
+   * and right sides.
+   *
+   * @param intakeSimple The position to intake at
+   * @param overBumpAllianceAlt The position to go over the bump
+   * @param shootFromCorner The position to shoot at the hub from
+   */
+  public record AutoPositions(
+      Pose2d center,
+      Pose2d intakeSimple,
+      Pose2d overBumpNeutral,
+      Pose2d overBumpAlliance,
+      Pose2d overBumpAllianceAlt,
+      Pose2d shootFromCorner) {}
+
+  // The necessary poses for autos on left side
+  private final AutoPositions poseA =
+      new AutoPositions(
+          /* center */ new Pose2d(3.457, 4.941, new Rotation2d(Units.degreesToRadians(-55.305))),
+          /* intakeSimple */ new Pose2d(
+              7.7052903175354, 5.8276801109313965, Rotation2d.fromDegrees(0.0)),
+          /* overBumpNeutral */ new Pose2d(6.1, 5.574310302734375, Rotation2d.fromDegrees(0)),
+          /* overBumpAlliance */ new Pose2d(
+              3.0666706562042236, 5.574310302734375, Rotation2d.fromDegrees(0.0)),
+          /* overBumpAllianceAlt */ new Pose2d(
+              3.086160182952881, 5.437880039215088, Rotation2d.fromDegrees(0)),
+          /* shootFromCorner */ new Pose2d(
+              3.086160182952881, 5.437880039215088, Rotation2d.fromRadians(-0.7553977556351216)));
+
+  // The necessary poses for autos on right side
+  private final AutoPositions poseB =
+      new AutoPositions(
+          AllianceFlipUtil.reflectY(poseA.center),
+          AllianceFlipUtil.reflectY(poseA.intakeSimple),
+          AllianceFlipUtil.reflectY(poseA.overBumpNeutral),
+          AllianceFlipUtil.reflectY(poseA.overBumpAlliance),
+          AllianceFlipUtil.reflectY(poseA.overBumpAllianceAlt),
+          AllianceFlipUtil.reflectY(poseA.shootFromCorner));
+
+  // Relevant subsystems for the autos
   private final Drive drive;
   private final Orchestrator orchestrator;
   private final Intake intake;
   private final IntakeRollers rollers;
   private final Shooter shooter;
 
+  /**
+   * Basic constructor, takes in all initialized subsystems and stores them
+   *
+   * @param drive Drive subsystem
+   * @param orchestrator Orchestrator subsystem
+   * @param intake Intake subsystem
+   * @param rollers Intake rollers subsystem
+   * @param shooter Shooter subsystem
+   */
   public Autos(
       Drive drive,
       Orchestrator orchestrator,
@@ -37,42 +86,7 @@ public Autos(
     this.shooter = shooter;
   }
 
-  private static final Supplier<Pose2d> climb =
-      () -> new Pose2d(1.583, 3.750, new Rotation2d(Units.degreesToRadians(180.000)));
   private static final Supplier<Pose2d> center = () -> new Pose2d(3.504, 4.019, new Rotation2d(0));
-  private static final Supplier<Pose2d> CenterA =
-      () -> new Pose2d(3.457, 4.941, new Rotation2d(Units.degreesToRadians(-55.305)));
-  private static final Supplier<Pose2d> intakeSimplePoseA =
-      () -> new Pose2d(7.7052903175354, 5.8276801109313965, Rotation2d.fromDegrees(0.0));
-  private static final Supplier<Pose2d> overBumpNeutralPoseA =
-      () -> new Pose2d(6.1, 5.574310302734375, Rotation2d.fromDegrees(0));
-
-  private static final Supplier<Pose2d> intakeComplexFirstPoseA =
-      () -> new Pose2d(6.862, 6.877, Rotation2d.fromDegrees(-90.0));
-  // 6.862
-  private static final Supplier<Pose2d> intakeComplexSecondPoseA =
-      () -> new Pose2d(7.825, 6.877, Rotation2d.fromDegrees(-90));
-
-  private static final Supplier<Pose2d> intakeComplexThirdPoseA =
-      () -> new Pose2d(7.805, 4.461, Rotation2d.fromDegrees(-90));
-  private static final Supplier<Pose2d> overBumpAlliancePoseA =
-      () -> new Pose2d(3.0666706562042236, 5.574310302734375, Rotation2d.fromDegrees(0.0));
-  private static final Supplier<Pose2d> overBumpAllianceAltPoseA =
-      () -> new Pose2d(3.086160182952881, 5.437880039215088, Rotation2d.fromDegrees(0));
-  private static final Supplier<Pose2d> shootFromCornerPoseA =
-      () ->
-          new Pose2d(
-              3.086160182952881, 5.437880039215088, Rotation2d.fromRadians(-0.7553977556351216));
-  private static final Supplier<Pose2d> intakeSimplePoseB =
-      () -> new Pose2d(7.724780082702637, 2.436420202255249, Rotation2d.fromDegrees(0.0));
-  private static final Supplier<Pose2d> overBumpAlliancePoseB =
-      () -> new Pose2d(3.1251401901245117, 2.397440195083618, Rotation2d.fromDegrees(0.0));
-  private static final Supplier<Pose2d> overBumpAllianceAltPoseB =
-      () -> new Pose2d(3.1251401901245117, 2.397440195083618, Rotation2d.fromDegrees(0));
-  private static final Supplier<Pose2d> shootFromCornerPoseB =
-      () ->
-          new Pose2d(
-              3.1251401901245117, 2.397440195083618, Rotation2d.fromRadians(0.7358299996216245));
   private static final Supplier<Pose2d> depotPoseStopPoint =
       () -> new Pose2d(1.7998201847076416, 5.983600616455078, Rotation2d.fromDegrees(180));
   private static final Supplier<Pose2d> depotPose =
@@ -93,7 +107,13 @@ public Autos(
           AllianceFlipUtil.apply(
               new Pose2d(3.256, 5.665, new Rotation2d(Units.degreesToRadians(115.83 + 180))));
 
-  // Helper functions that extract common code for pathplanner atuos
+  /**
+   * Helper function for a single cycle auto with a constant shooting speed
+   *
+   * @param path The pathplanner path file name to use
+   * @param startPose A supplier that returns that starting position of the robot for this path
+   * @return A command that follows the path and shoots
+   */
   private Command ppCycle(String path, Supplier<Pose2d> startPose) {
     return Commands.sequence(
         Commands.runOnce(() -> drive.setPose(startPose.get())),
@@ -110,6 +130,13 @@ private Command ppCycle(String path, Supplier<Pose2d> startPose) {
             orchestrator.feedUp()));
   }
 
+  /**
+   * Helper function for a single cycle auto that uses shooter regression to shoot into the hub
+   *
+   * @param path The pathplanner path file name to use
+   * @param startPose A Supplier that returns the starting position of the robot for this path
+   * @return A command that follows the path and shoots
+   */
   private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
     return Commands.sequence(
         Commands.runOnce(() -> drive.setPose(startPose.get())),
@@ -130,6 +157,16 @@ private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
             orchestrator.feedUp()));
   }
 
+  /**
+   * Helper function for a double cycle auto that uses shooter regression to shoot into the hub
+   * twice (once after each cycle)
+   *
+   * @param path1 The pathplanner path file name to use for the first cycle
+   * @param path2 The pathplanner path file name to use for the second cycle
+   * @param startPose The starting position of the robot for the first path and the position it ends
+   *     up in after the path
+   * @return A command that follows the paths and shoots
+   */
   private Command pp2CycleRegression(
       String path1, String path2, Supplier<Pose2d> startPose, Supplier<Pose2d> startPose2) {
     return ppCycleRegression(path1, startPose)
@@ -137,72 +174,131 @@ private Command pp2CycleRegression(
         .andThen(ppCycleRegression(path2, startPose2));
   }
 
-  // Pathplanner autos using the helper functions
+  /**
+   * Left side pathplanner auto for a single cycle with fixed shooting distance
+   *
+   * @return A command that executes the auto
+   */
   public Command ppACycleLeft() {
     return ppCycle("A-Cycle-LeftSweep", startAside);
   }
 
+  /**
+   * Right side pathplanner auto for a single cycle with fixed shooting distance
+   *
+   * @return A command that executes the auto
+   */
   public Command ppBCycleRight() {
     return ppCycle("B-Cycle-RightSweep", startBside);
   }
 
+  /**
+   * Left side pathplanner auto for a single cycle that supports any shooting distance
+   *
+   * @return A command that executes the auto
+   */
   public Command ppB2CycleRightRegression() {
     return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside, connectBside);
   }
 
+  /**
+   * Left side pathplanner auto for 2 cycles that support any shooting distance
+   *
+   * @return A comand that executes the auto
+   */
   public Command ppA2CycleRightRegression() {
     return pp2CycleRegression("A-Cycle1", "A-Cycle2", startAside, connectAside);
   }
 
+  /**
+   * Right side pathplanner auto for a single cycle that supports any shooting distance
+   *
+   * @return A command that executes the auto
+   */
   public Command ppBCycleRightRegression() {
     return ppCycleRegression("B-Cycle-RightSweep", startBside);
   }
 
+  /**
+   * Left side pathplanner auto for a single cycle that supports any shooting distance
+   *
+   * @return A command that executes the auto
+   */
   public Command ppACycleLeftRegression() {
     return ppCycleRegression("A-Cycle-LeftSweep", startAside);
   }
 
-  // Misc helper functions for manual autos
-  private Command shootPullIntake() {
-    return Commands.parallel(
-        orchestrator.spinUpShooterHub(),
-        orchestrator.feedUp(),
-        Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0)));
-  }
-
+  /**
+   * Gets relevant poses for the autos
+   *
+   * @param isA Whether to get the poses for A path or B path (true for A, false for B)
+   * @return The relevant poses for the calling auto
+   */
   AutoPositions getPoses(boolean isA) {
     if (isA) {
-      return new AutoPositions(
-          intakeSimplePoseA.get(), overBumpAllianceAltPoseA.get(), shootFromCornerPoseA.get());
+      return poseA;
     }
-    return new AutoPositions(
-        intakeSimplePoseB.get(), overBumpAllianceAltPoseB.get(), shootFromCornerPoseB.get());
+    return poseB;
   }
 
-  // Helper functions that extract common code for manual autos
+  /**
+   * A helper function for a manual, straightDriveToPose auto that does one cycle with support for
+   * any shooting distance
+   *
+   * @param isA Whether to get the poses for A path or B path (true for A, false for B)
+   * @return A command that calls the auto
+   */
   private Command ccManualAuto(boolean isA) {
     AutoPositions poses = getPoses(isA);
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose))
-                .withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimple)).withTimeout(5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt))
             .withTimeout(5),
         rollers.stopIntakeCommand().withTimeout(0.05),
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()),
-        shootPullIntake());
+        Commands.parallel(
+            orchestrator.spinUpShooterHub(),
+            orchestrator.feedUp(),
+            Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))));
   }
 
+  /**
+   * Left side straight drive to pose auto that does one cycle with support for any shooting
+   * distance
+   *
+   * @return A command that calls the auto
+   */
+  public Command ACCManualAuto() {
+    return ccManualAuto(true);
+  }
+
+  /**
+   * Right side straight drive to pose auto that does one cycle with support for any shooting
+   * distance
+   *
+   * @return A command that calls the auto
+   */
+  public Command BCCManualAuto() {
+    return ccManualAuto(false);
+  }
+
+  /**
+   * A helper function for a manual, straightDriveToPose auto that does one cycle with fixed
+   * shooting distance
+   *
+   * @param isA Whether to get the poses for A path or B path (true for A, false for B)
+   * @return A command that calls the auto
+   */
   private Command ccManualAutoAlt(boolean isA) {
     AutoPositions poses = getPoses(isA);
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose))
-                .withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimple)).withTimeout(5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt))
             .withTimeout(3.5),
@@ -217,13 +313,37 @@ private Command ccManualAutoAlt(boolean isA) {
             Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))));
   }
 
+  /**
+   * Left side straight drive to pose auto that does one cycle with fixed shooting distance
+   *
+   * @return A command that calls the auto
+   */
+  public Command ACCManuelAutoAlt() {
+    return ccManualAutoAlt(true);
+  }
+
+  /**
+   * Right side straight drive to pose auto that does one cycle with fixed shooting distance
+   *
+   * @return A command that calls the auto
+   */
+  public Command BCCManuelAutoAlt() {
+    return ccManualAutoAlt(false);
+  }
+
+  /**
+   * A helper function for a manual, straightDriveToPose auto that does one cycle with fixed
+   * shooting distance and then goes back to pick up more balls from the center.
+   *
+   * @param isA Whether to get the poses for A path or B path (true for A, false for B)
+   * @return A command that calls the auto
+   */
   private Command ccManualAutoOverBump(boolean isA) {
     AutoPositions poses = getPoses(isA);
 
     return Commands.sequence(
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose))
-                .withTimeout(5),
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimple)).withTimeout(5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)),
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt))
             .withTimeout(3.5),
@@ -242,91 +362,40 @@ private Command ccManualAutoOverBump(boolean isA) {
         new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt))
             .withTimeout(2),
         Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimplePose))
+            new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimple))
                 .withTimeout(3.5),
             intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
   }
 
-  // Manual autos using the helper functions
-  public Command ACCManuelAuto() {
-    return ccManualAuto(true);
-  }
-
-  public Command BCCManuelAuto() {
-    return ccManualAuto(false);
-  }
-
-  public Command ACCManuelAutoAlt() {
-    return ccManualAutoAlt(true);
-  }
-
-  public Command BCCManuelAutoAlt() {
-    return ccManualAutoAlt(false);
-  }
-
-  public Command ACCManuelAutoOverBump() {
+  /**
+   * Left side straight drive to pose auto that does one cycle with fixed shooting distance and then
+   * goes back to pick up more balls from the center.
+   *
+   * @return A command that cals the auto
+   */
+  public Command ACCManualAutoOverBump() {
     return ccManualAutoOverBump(true);
   }
 
-  public Command BCCManuelAutoOverBump() {
+  /**
+   * Right side straight drive to pose auto that does one cycle with fixed shooting distance and
+   * then goes back to pick up more balls from the center.
+   *
+   * @return A command that cals the auto
+   */
+  public Command BCCManualAutoOverBump() {
     return ccManualAutoOverBump(false);
   }
 
-  // Other manual autos
+  /**
+   * Center auto that shoots the 8 balls we preload with and nothing else
+   *
+   * @return A command that calls the auto
+   */
   public Command EightBalls() {
     return Commands.sequence(
         Commands.deadline(
             orchestrator.driveToHub().withTimeout(3.0), orchestrator.spinUpShooterHub()),
         Commands.parallel(orchestrator.spinUpShooterHub(), orchestrator.feedUp()));
   }
-
-  public Command ACCManuelImprovedComplexIntake() {
-    return Commands.sequence(
-        Commands.deadline(
-            Commands.sequence(
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpNeutralPoseA.get()))
-                    .withTimeout(2),
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexFirstPoseA.get())),
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexSecondPoseA.get()))
-                    .withTimeout(1.9),
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeComplexThirdPoseA.get()))
-                    .withTimeout(3),
-                new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpNeutralPoseA.get()))),
-            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
-            .withTimeout(3.5),
-        rollers.stopIntakeCommand().withTimeout(0.05),
-        Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
-                .withTimeout(4),
-            orchestrator.spinUpShooter(1240)),
-        shootPullIntake());
-  }
-
-  public Command ADepot() {
-    return Commands.sequence(
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPoseStopPoint.get()))
-            .withTimeout(2),
-        Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPose.get())).withTimeout(3.5),
-            Commands.parallel(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))),
-        rollers.stopIntakeCommand().withTimeout(0.05),
-        Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(shootFromCornerPoseA.get()))
-                .withTimeout(2),
-            orchestrator.spinUpShooter(1250)),
-        Commands.deadline(
-            Commands.waitSeconds(5),
-            orchestrator.spinUpShooter(1250),
-            orchestrator.feedUp(),
-            Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))),
-        rollers.stopIntakeCommand().withTimeout(0.05),
-        shooter.stop(),
-        new DriveToPose(drive, () -> AllianceFlipUtil.apply(overBumpAllianceAltPoseA.get()))
-            .withTimeout(2),
-        Commands.deadline(
-            new DriveToPose(drive, () -> AllianceFlipUtil.apply(intakeSimplePoseA.get()))
-                .withTimeout(3.5),
-            intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)));
-  }
 }
diff --git a/src/main/java/frc/robot/commands/auto/README.md b/src/main/java/frc/robot/commands/auto/README.md
new file mode 100644
index 000000000..91c964929
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/README.md
@@ -0,0 +1,10 @@
+# Autos
+
+This folder contains all the code that runs our autos and programs them. **Always sim your autos!**
+
+## Naming Scheme
+
+- "A" is left side
+- "B" is right side
+- "pp" stands for pathplanner autos
+- "manual" stands for an auto that uses drive to pose
diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java
deleted file mode 100644
index 8352349f6..000000000
--- a/src/main/java/frc/robot/subsystems/climber/Climber.java
+++ /dev/null
@@ -1,64 +0,0 @@
-package frc.robot.subsystems.climber;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import org.littletonrobotics.junction.Logger;
-
-public class Climber extends SubsystemBase {
-
-  public final ClimberIO io;
-  private final ClimberIOInputsAutoLogged inputs;
-  private boolean rotateToTarget = false;
-
-  public Climber(ClimberIO io) {
-    this.io = io;
-    inputs = new ClimberIOInputsAutoLogged();
-  }
-
-  @Override
-  public void periodic() {
-    io.updateInputs(inputs);
-    Logger.processInputs("Climber", inputs);
-    if (rotateToTarget) {
-      io.goToRotation();
-    }
-  }
-
-  public boolean atTargetRotation() {
-    return inputs.atTargetRotation;
-  }
-
-  public Command toRotation(double degrees) {
-    return Commands.run(
-            () -> {
-              rotateToTarget = true;
-              io.setRotation(degrees);
-            },
-            this)
-        .until(() -> inputs.atTargetRotation)
-        .withName("toRotation");
-  }
-
-  public Command runPercent(double percent) {
-    return Commands.run(
-            () -> {
-              rotateToTarget = false;
-              io.setPercent(percent);
-            },
-            this)
-        .withName("runPercent");
-  }
-
-  public double getPositionDegrees() {
-    return inputs.positionDegrees;
-  }
-
-  public boolean isCalibrated() {
-    return inputs.isCalibrated;
-  }
-
-  public boolean limitSwitchPressed() {
-    return inputs.limitSwitch;
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java
deleted file mode 100644
index c4a775836..000000000
--- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java
+++ /dev/null
@@ -1,23 +0,0 @@
-package frc.robot.subsystems.climber;
-
-public class ClimberConstants {
-  public static final int CLIMBER_CURRENT_LIMIT = 80;
-  public static final int CLIMBER_SUPPLY_CURRENT_LIMIT = 40;
-
-  public static final double ENCODER_CONVERSION_FACTOR = 360.0;
-
-  public static final double STARTING_DEGREES = 0.0;
-
-  // PID constants
-  public static final double CLIMBER_KP = 1.0;
-  public static final double CLIMBER_KI = 0.0;
-  public static final double CLIMBER_KD = 0.0;
-
-  public static final double TOLERANCE = 1.0; // measured in degrees
-
-  // Calibration constants
-  public static final int LIMIT_SWITCH_ID = 3;
-  public static final double CALIBRATION_POSITION_DEGREES =
-      0.0; // position when limit switch is hit
-  public static final double CALIBRATION_PERCENT = -0.15; // slow speed to find limit switch
-}
diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java
deleted file mode 100644
index c162944a1..000000000
--- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java
+++ /dev/null
@@ -1,25 +0,0 @@
-package frc.robot.subsystems.climber;
-
-import org.littletonrobotics.junction.AutoLog;
-
-public interface ClimberIO {
-  @AutoLog
-  class ClimberIOInputs {
-    public double positionDegrees = 0.0;
-    public double velocityDegreesPerSec = 0.0;
-    public double appliedVolts = 0.0;
-    public double currentAmps = 0.0;
-    public double targetRotation = 0.0;
-    public boolean atTargetRotation = false;
-    public boolean limitSwitch = false;
-    public boolean isCalibrated = false;
-  }
-
-  default void updateInputs(ClimberIOInputs inputs) {}
-
-  default void setPercent(double percent) {}
-
-  default void setRotation(double degrees) {}
-
-  default void goToRotation() {}
-}
diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOPhysical.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOPhysical.java
deleted file mode 100644
index e57a1bebd..000000000
--- a/src/main/java/frc/robot/subsystems/climber/ClimberIOPhysical.java
+++ /dev/null
@@ -1,98 +0,0 @@
-package frc.robot.subsystems.climber;
-
-import static frc.lib.utils.PhoenixUtil.*;
-import static frc.robot.Schematic.climberMotorCanId;
-import static frc.robot.subsystems.climber.ClimberConstants.*;
-
-import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VoltageOut;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.units.measure.*;
-import edu.wpi.first.wpilibj.DigitalInput;
-
-public class ClimberIOPhysical implements ClimberIO {
-  private final TalonFX talon;
-  private final DigitalInput limitSwitch;
-
-  private final StatusSignal<Angle> position;
-  private final StatusSignal<AngularVelocity> velocity;
-  private final StatusSignal<Voltage> appliedVolts;
-  private final StatusSignal<Current> current;
-
-  private final VoltageOut voltageRequest = new VoltageOut(0);
-  private final PositionVoltage positionRequest = new PositionVoltage(0);
-
-  private boolean isCalibrated = false;
-  private double targetRotation;
-
-  public ClimberIOPhysical() {
-    talon = new TalonFX(climberMotorCanId);
-    limitSwitch = new DigitalInput(LIMIT_SWITCH_ID);
-
-    var config = new TalonFXConfiguration();
-    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-    config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-    config.CurrentLimits.StatorCurrentLimit = CLIMBER_CURRENT_LIMIT;
-    config.CurrentLimits.StatorCurrentLimitEnable = true;
-    config.CurrentLimits.SupplyCurrentLimit = CLIMBER_SUPPLY_CURRENT_LIMIT;
-    config.CurrentLimits.SupplyCurrentLimitEnable = true;
-    config.Slot0.kP = CLIMBER_KP;
-    config.Slot0.kI = CLIMBER_KI;
-    config.Slot0.kD = CLIMBER_KD;
-
-    tryUntilOk(5, () -> talon.getConfigurator().apply(config));
-    tryUntilOk(5, () -> talon.setPosition(STARTING_DEGREES / ENCODER_CONVERSION_FACTOR));
-
-    position = talon.getPosition();
-    velocity = talon.getVelocity();
-    appliedVolts = talon.getMotorVoltage();
-    current = talon.getStatorCurrent();
-
-    BaseStatusSignal.setUpdateFrequencyForAll(50.0, position, velocity, appliedVolts, current);
-  }
-
-  @Override
-  public void updateInputs(ClimberIOInputs inputs) {
-    BaseStatusSignal.refreshAll(position, velocity, appliedVolts, current);
-
-    inputs.positionDegrees = position.getValueAsDouble() * ENCODER_CONVERSION_FACTOR;
-    inputs.velocityDegreesPerSec = velocity.getValueAsDouble() * ENCODER_CONVERSION_FACTOR;
-    inputs.appliedVolts = appliedVolts.getValueAsDouble();
-    inputs.currentAmps = current.getValueAsDouble();
-    inputs.targetRotation = targetRotation;
-    inputs.atTargetRotation =
-        isCalibrated && Math.abs(targetRotation - inputs.positionDegrees) < TOLERANCE;
-    inputs.limitSwitch = !limitSwitch.get();
-
-    if (inputs.limitSwitch && !this.isCalibrated) {
-      this.isCalibrated = true;
-      tryUntilOk(
-          5, () -> talon.setPosition(CALIBRATION_POSITION_DEGREES / ENCODER_CONVERSION_FACTOR));
-    }
-    inputs.isCalibrated = this.isCalibrated;
-  }
-
-  @Override
-  public void setPercent(double percent) {
-    talon.setControl(voltageRequest.withOutput(percent * 12.0));
-  }
-
-  @Override
-  public void setRotation(double degrees) {
-    this.targetRotation = degrees;
-  }
-
-  @Override
-  public void goToRotation() {
-    if (!isCalibrated) {
-      setPercent(CALIBRATION_PERCENT);
-    } else {
-      talon.setControl(positionRequest.withPosition(targetRotation / ENCODER_CONVERSION_FACTOR));
-    }
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java
deleted file mode 100644
index 47fa0aaac..000000000
--- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java
+++ /dev/null
@@ -1,3 +0,0 @@
-package frc.robot.subsystems.climber;
-
-public class ClimberIOSim {}
diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java
index c36fdc122..e6f7845db 100644
--- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java
+++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java
@@ -11,11 +11,21 @@ public class MagicCarpet extends SubsystemBase {
   private final MagicCarpetIOInputsAutoLogged inputs = new MagicCarpetIOInputsAutoLogged();
   public boolean manualRun;
 
+  /**
+   * Initializes the magic carpet
+   *
+   * @param io The implementation of the hardware
+   */
   public MagicCarpet(MagicCarpetIO io) {
     this.io = io;
     this.manualRun = false;
   }
 
+  /**
+   * Runs the magic carpet at a constant speed
+   *
+   * @return A command to run the magic carpet at a constant speed
+   */
   public Command run() {
     return setManualControl(true)
         .andThen(Commands.run(() -> io.setSpeed(MagicCarpetConstants.BELT_SPEED), this))
@@ -23,6 +33,11 @@ public Command run() {
         .withName("start");
   }
 
+  /**
+  * Control whether to run magic carpet through commands or periodic
+  *
+  * @param manual Call run manually instead of through periodic
+  */
   public Command setManualControl(boolean manual) {
     return Commands.runOnce(() -> this.manualRun = manual, this);
   }
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 8ff49c4e8..3d18d1c9a 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -1,5 +1,6 @@
 package frc.robot.subsystems.shooter;
 
+import static edu.wpi.first.units.Units.*;
 import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Volts;
 import static frc.robot.subsystems.shooter.ShooterConstants.TOLERANCE;
@@ -7,6 +8,10 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.units.*;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -14,7 +19,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
 import frc.robot.RobotState;
-import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
 public class Shooter extends SubsystemBase {
@@ -23,7 +28,7 @@ public class Shooter extends SubsystemBase {
   private final SysIdRoutine sysId;
 
   public boolean controllerEnabled = true;
-  private double targetRadPerSec = 0.0;
+  private AngularVelocity targetSpeed = RadiansPerSecond.of(0);
   private double rampedTarget = 0.0;
 
   // Feedforward: handles steady-state voltage (V = KS + KV * velocity)
@@ -37,6 +42,11 @@ public class Shooter extends SubsystemBase {
   // This prevents current spikes that cause oscillation with a 20A limit
   private final SlewRateLimiter targetRamper = new SlewRateLimiter(800);
 
+  /**
+   * Initializes the shooter with a Shooter IO
+   *
+   * @param io A ShooterIO implementing instance
+   */
   public Shooter(ShooterIO io) {
     this.io = io;
     sysId =
@@ -52,10 +62,11 @@ public Shooter(ShooterIO io) {
   @Override
   public void periodic() {
     io.updateInputs(inputs);
-    RobotState.getInstance().shooterAtSpeed = isAtSetpoint() && targetRadPerSec > 0;
+    RobotState.getInstance().shooterAtSpeed =
+        isAtSetpoint() && targetSpeed.gt(RadiansPerSecond.of(0));
     if (controllerEnabled) {
       // Ramp toward the target to avoid current spikes
-      rampedTarget = targetRamper.calculate(targetRadPerSec);
+      rampedTarget = targetRamper.calculate(targetSpeed.in(RadiansPerSecond));
 
       double ff = feedforward.calculate(rampedTarget);
       double pidOutput = pid.calculate(inputs.shooterWheelVelocityRadPerSec, rampedTarget);
@@ -70,7 +81,7 @@ public void periodic() {
       Logger.recordOutput("Shooter/PIDVoltage", pidOutput);
       Logger.recordOutput("Shooter/CommandedVoltage", voltage);
       Logger.recordOutput("Shooter/RampedTargetRadPerSec", rampedTarget);
-      Logger.recordOutput("Shooter/Setpoint", targetRadPerSec);
+      Logger.recordOutput("Shooter/Setpoint", targetSpeed.in(RadiansPerSecond));
     }
 
     Logger.processInputs("Shooter", inputs);
@@ -78,27 +89,49 @@ public void periodic() {
     RobotState.getInstance().shooterAtSpeed = isAtSetpoint();
   }
 
+  /**
+   * Runs characterization voltage (used for FF)
+   *
+   * @param voltage The voltage to apply
+   */
   public void runCharacterization(double voltage) {
     io.setVoltage(voltage);
   }
 
+  /**
+   * Runs SysID Quasistatic for feed forward
+   *
+   * @param direction The direction to run it in
+   * @return A command that runs SysID Quasistatic
+   */
   public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
     return Commands.runOnce(() -> controllerEnabled = false, this)
         .andThen(run(() -> runCharacterization(0.0)).withTimeout(1.0))
         .andThen(sysId.quasistatic(direction));
   }
 
+  /**
+   * Runs SysID Quasistatic for feed forward
+   *
+   * @param direction The direction to run it in
+   * @return A command that runs SysID Dynamic
+   */
   public Command sysIdDynamic(SysIdRoutine.Direction direction) {
     return Commands.runOnce(() -> controllerEnabled = false, this)
         .andThen(run(() -> runCharacterization(0.0)).withTimeout(1.0))
         .andThen(sysId.dynamic(direction));
   }
 
+  /**
+   * Stop the shooter
+   *
+   * @return A command that stops the shooter
+   */
   public Command stop() {
     return Commands.runOnce(
             () -> {
               controllerEnabled = false;
-              targetRadPerSec = 0.0;
+              targetSpeed = RadiansPerSecond.of(0);
               rampedTarget = 0.0;
               targetRamper.reset(0.0);
               io.stop();
@@ -107,81 +140,100 @@ public Command stop() {
         .withName("shooter_stop_please");
   }
 
+  /**
+   * Sets raw voltage for the shooter
+   *
+   * @param volts The volts amount to set it to (max 12)
+   * @return A Command that sets the shooter voltage
+   */
   public Command setVoltage(double volts) {
     return Commands.runOnce(() -> controllerEnabled = false, this)
         .andThen(Commands.run(() -> io.setVoltage(volts), this));
   }
 
-  public Command setTargetVelocityRPM(double rpm) {
-    return Commands.run(
-            () -> {
-              targetRadPerSec = ((rpm / 2 * Math.PI) * 60.0);
-              controllerEnabled = true;
-            },
-            this)
-        .withName("setTargetVelocityRPM");
-  }
-
-  public Command setTargetVelocityRadians(double radPerSec) {
+  /**
+   * Set the target velocity of the shooter
+   *
+   * @param velocity A supplier that returns the target velocity of the shooter
+   * @return A command that sets the target velocity of the shooter to the value returned by the
+   *     supplier
+   */
+  public Command setTargetVelocity(Supplier<AngularVelocity> velocity) {
     return Commands.run(
             () -> {
-              targetRadPerSec = radPerSec;
+              targetSpeed = velocity.get();
               controllerEnabled = true;
             },
             this)
-        .withName("setTargetVelocityRadians");
+        .withName("setTargetVelocity");
   }
 
-  public Command setTargetVelocityRadians(DoubleSupplier radPerSec) {
+  /**
+   * Set the target velocity of the shooter
+   *
+   * @param velocity The target velocity of the shooter
+   * @return A command that sets the target velocity of the shooter to the specified value
+   */
+  public Command setTargetVelocity(AngularVelocity velocity) {
     return Commands.run(
             () -> {
-              targetRadPerSec = radPerSec.getAsDouble();
+              targetSpeed = velocity;
               controllerEnabled = true;
             },
             this)
-        .withName("setTargetVelocityRadians");
+        .withName("setTargetVelocity");
   }
 
-  /**
-   * Continuously re-asserts shooter target while scheduled.
-   *
-   * <p>Useful for toggle/manual control flows where another command may temporarily disable the
-   * shooter controller.
-   */
-  public Command setTargetVelocityRadiansRepeatedly(double radPerSec) {
+  public Command setTargetVelocityRepeatedly(AngularVelocity velocity) {
     return Commands.repeatingSequence(
             Commands.runOnce(
                 () -> {
-                  targetRadPerSec = radPerSec;
+                  targetSpeed = velocity;
                   controllerEnabled = true;
                 },
                 this),
             Commands.waitSeconds(0.02))
-        .withName("setTargetVelocityRadiansRepeatedly");
+        .withName("setTargetVelocityRepeatedly");
   }
 
+  /**
+   * Check if the shooter is at setpoint (+- TOLERANCE)
+   *
+   * @return A boolean asserting whether the shooter is at setpoint (true for yes)
+   */
   public boolean isAtSetpoint() {
-    return Math.abs(inputs.shooterWheelVelocityRadPerSec - (targetRadPerSec)) < TOLERANCE;
+    return Math.abs(inputs.shooterWheelVelocityRadPerSec - (targetSpeed.in(RadiansPerSecond)))
+        < TOLERANCE;
   }
 
   // TODO: empirically determine the relationship between distance and air time
-  public double getAirTimeSeconds(DoubleSupplier distance) {
-    return 0.0617 * distance.getAsDouble() + 0.872;
+  public Time getAirTimeSeconds(Distance distance) {
+    return Seconds.of(0.0617 * distance.in(Meters) + 0.872);
   }
 
   // TODO: empirically determine the relationship between distance and shooter velocity
-
-  public DoubleSupplier calculateSetpoint(DoubleSupplier distance) {
+  /**
+   * Calculates a setpoint (in radians per second) based on distance
+   *
+   * @param distance Distance
+   * @return Setpoint in radians per second
+   */
+  public Supplier<AngularVelocity> calculateSetpoint(Supplier<Distance> distance) {
     // calculate rad/s depending on distance
     return () -> {
-      double setpoint = 183.35 * distance.getAsDouble() + 750.93;
-      if (setpoint > 5000) {
-        return 5000;
-      } else return setpoint;
+      AngularVelocity velocity = RadiansPerSecond.of(183.35 * distance.get().in(Meters) + 750.93);
+      if (velocity.gt(RadiansPerSecond.of(5000))) {
+        return RadiansPerSecond.of(5000);
+      } else return velocity;
     };
   }
 
-  public double getSetpoint() {
-    return targetRadPerSec;
+  /**
+   * Get the current target velocity of the shooter
+   *
+   * @return The target velocity of the shooter
+   */
+  public AngularVelocity getSetpoint() {
+    return targetSpeed;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
index c8a389d04..b019e9229 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
@@ -134,8 +134,4 @@ public void stop() {
     bottomLeftMotor.set(0);
     bottomRightMotor.set(0);
   }
-
-  private double distanceToRPM(double distanceMeters) {
-    return distanceMeters * 2.79775342767 + 16.8379527141;
-  }
 }
diff --git a/src/main/java/frc/robot/util/ShooterLeadCompensator.java b/src/main/java/frc/robot/util/ShooterLeadCompensator.java
index 2405d0979..ccaecb1dc 100644
--- a/src/main/java/frc/robot/util/ShooterLeadCompensator.java
+++ b/src/main/java/frc/robot/util/ShooterLeadCompensator.java
@@ -1,8 +1,12 @@
 package frc.robot.util;
 
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Seconds;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.measure.Time;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterConstants;
@@ -49,8 +53,8 @@ public ShootWhileDrivingResult shootWhileDriving(Translation2d targetPosition) {
      */
     for (int i = 0; i < 10; i++) {
       double distance = aimPoint.minus(shooterPos).getNorm();
-      double t = shooter.getAirTimeSeconds(() -> distance);
-      aimPoint = targetPosition.plus(v.times(t));
+      Time t = shooter.getAirTimeSeconds(Meters.of(distance));
+      aimPoint = targetPosition.plus(v.times(t.in(Seconds)));
     }
 
     double finalDistance = aimPoint.minus(shooterPos).getNorm();

From 13f6a917737c1e6bf33378d52593b1b379e55fb4 Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Sat, 11 Apr 2026 21:18:30 -0400
Subject: [PATCH 066/102] Fix formatting

---
 .../frc/robot/subsystems/magicCarpet/MagicCarpet.java     | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java
index e6f7845db..3b49a1522 100644
--- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java
+++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java
@@ -34,10 +34,10 @@ public Command run() {
   }
 
   /**
-  * Control whether to run magic carpet through commands or periodic
-  *
-  * @param manual Call run manually instead of through periodic
-  */
+   * Control whether to run magic carpet through commands or periodic
+   *
+   * @param manual Call run manually instead of through periodic
+   */
   public Command setManualControl(boolean manual) {
     return Commands.runOnce(() -> this.manualRun = manual, this);
   }

From d7af5c7b0ef34642f771170ee5cad3528a4821cb Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sat, 11 Apr 2026 21:11:23 -0500
Subject: [PATCH 067/102] tuned shooter

---
 src/main/java/frc/robot/RobotContainer.java   | 24 +++++++------
 .../subsystems/intake/IntakeConstants.java    |  6 ++--
 .../intake/extend/IntakeExtend.java           |  4 +++
 .../intake/extend/IntakeExtendIO.java         |  3 ++
 .../intake/extend/IntakeExtendIOSparkMax.java | 35 ++++++++++++++-----
 .../frc/robot/subsystems/shooter/Shooter.java |  2 +-
 .../subsystems/shooter/ShooterConstants.java  |  4 +--
 7 files changed, 54 insertions(+), 24 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 151a6d629..00939a83a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -256,7 +256,8 @@ public RobotContainer() {
     autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
 
     autoChooser.addDefaultOption("Do Nothing", Commands.none());
-
+    autoChooser.addOption(
+        "RunIntakeOut", intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
     autoChooser.addOption("Manual A-CC", autos.ACCManualAuto());
     autoChooser.addOption("Manual A-CC-Improved", autos.ACCManuelAutoAlt());
     autoChooser.addOption("Manual A-CC-Over-Bump", autos.ACCManualAutoOverBump());
@@ -330,8 +331,12 @@ private void configureButtonBindings() {
                 .ignoringDisable(true));
     new Trigger(() -> driverController.getHID().getPOV() != -1)
         .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV()));
-    driverController.x().toggleOnTrue(orchestrator.aimToHub());
-    driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
+    //    driverController.x().toggleOnTrue(orchestrator.aimToHub());
+    //
+    // driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
+    driverController.x().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(20)));
+    driverController.y().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(60)));
+    driverController.a().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(110)));
     driverController
         .leftBumper()
         .and(operatorController.pov(180))
@@ -350,13 +355,12 @@ private void configureButtonBindings() {
 
     //     VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
     driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
-    //    driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
-    driverController.rightTrigger(0.1).toggleOnTrue(indexer.run());
+    driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
 
-    driverController
-        .a()
-        .and(operatorController.pov(180))
-        .onTrue(intakeExtend.resetExtendPosition());
+    //    driverController
+    //        .a()
+    //        .and(operatorController.pov(180))
+    //        .onTrue(intakeExtend.resetExtendPosition());
     driverController
         .rightBumper()
         .and(() -> !operatorController.pov(180).getAsBoolean())
@@ -378,7 +382,7 @@ private void configureButtonBindings() {
         .toggleOnTrue(orchestrator.spinUpShooterHub());
     operatorController.leftTrigger(0.1).toggleOnTrue(shooter.setVoltage(4));
     operatorController.y().whileTrue(indexer.reverse());
-    //    operatorController.x().whileTrue(intake.outtake());
+    operatorController.x().whileTrue(intakeRollers.outtake());
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index ee3abab23..9400c56cc 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -10,13 +10,13 @@ public class IntakeConstants {
 
   public static final double STALL_TIME = 0.1;
   public static final double STALL_VELOCITY = 0.1;
-  public static final double INTAKE_VOLTS = 12;
-  public static final double OUTTAKE_VOLTS = -10;
+  public static final double INTAKE_VOLTS = 8;
+  public static final double OUTTAKE_VOLTS = -12;
   public static final double EXTEND_VOLTS = 0.01;
   public static final double COLLAPSE_VOLTS = -0.01;
   public static final double INTAKE_MAX_VELOCITY = 200;
 
-  public static final int EXTEND_LIMIT_ID = 1;
+  public static final int EXTEND_LIMIT_ID = 0;
 
   public static final double PID_P = 0.023; // arbritrary values
   public static final double PID_I = 0.000004;
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index 2b0621a3b..4b1044e1d 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -70,6 +70,10 @@ public DoubleSupplier getAngle() {
     return () -> inputs.getExtendPos;
   }
 
+  public void setIdleMode(boolean idleMode) {
+    io.setIdleMode(idleMode);
+  }
+
   public Command setPose(double pose) {
     return Commands.runOnce(() -> io.resetExtendEncoder(pose), this);
   }
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java
index 082317d7c..706131aab 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java
@@ -35,6 +35,9 @@ default void goToPos(double pos) {}
 
   default void setPIDEnabled(boolean enabled) {}
 
+  default void setIdleMode(boolean cost) {}
+  ;
+
   default boolean getPIDEnabled() {
     return false;
   }
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java
index ed688a961..c21bc77d9 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java
@@ -33,6 +33,16 @@ public class IntakeExtendIOSparkMax implements IntakeExtendIO {
 
   public IntakeExtendIOSparkMax() {
     extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless);
+
+    pidController = extendMotor.getClosedLoopController();
+    extendMotorEncoder = extendMotor.getEncoder();
+
+    extendMotor.configure(
+        getSparkMaxConfig(), ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID);
+  }
+
+  public SparkMaxConfig getSparkMaxConfig() {
     EncoderConfig extendEnc = new EncoderConfig();
     extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION);
     extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION);
@@ -46,13 +56,7 @@ public IntakeExtendIOSparkMax() {
         .pid(PID_P, PID_I, PID_D)
         .feedbackSensor(FeedbackSensor.kPrimaryEncoder);
     extendConfig.encoder.apply(extendEnc);
-
-    pidController = extendMotor.getClosedLoopController();
-    extendMotorEncoder = extendMotor.getEncoder();
-
-    extendMotor.configure(
-        extendConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-    collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID);
+    return extendConfig;
   }
 
   @Override
@@ -61,7 +65,7 @@ public void updateInputs(IntakeExtendIOInputs inputs) {
     inputs.extendVelocity = extendMotorEncoder.getVelocity();
     inputs.extendVolts = extendMotor.getAppliedOutput();
     inputs.extendAmps = extendMotor.getOutputCurrent();
-    inputs.isCollapsed = false;
+    inputs.isCollapsed = isCollapsed();
     inputs.getExtendPos = extendMotorEncoder.getPosition();
     inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint() && usingPID;
     inputs.hasSetpoint = usingPID;
@@ -100,6 +104,21 @@ public void setPIDEnabled(boolean enabled) {
     this.usingPID = enabled;
   }
 
+  @Override
+  public void setIdleMode(boolean coast) {
+    if (coast) {
+      extendMotor.configure(
+          getSparkMaxConfig().idleMode(IdleMode.kCoast),
+          ResetMode.kResetSafeParameters,
+          PersistMode.kNoPersistParameters);
+    } else {
+      extendMotor.configure(
+          getSparkMaxConfig().idleMode(IdleMode.kBrake),
+          ResetMode.kResetSafeParameters,
+          PersistMode.kNoPersistParameters);
+    }
+  }
+
   @Override
   public void resetExtendEncoder(double position) {
     extendMotorEncoder.setPosition(position);
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 3d18d1c9a..0bbe8abce 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -40,7 +40,7 @@ public class Shooter extends SubsystemBase {
 
   // Slew rate limiter: ramps the target velocity gradually (rad/s per second)
   // This prevents current spikes that cause oscillation with a 20A limit
-  private final SlewRateLimiter targetRamper = new SlewRateLimiter(800);
+  private final SlewRateLimiter targetRamper = new SlewRateLimiter(1600);
 
   /**
    * Initializes the shooter with a Shooter IO
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 7db7ddb98..e6873cc40 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -13,9 +13,9 @@ public class ShooterConstants {
   public static final int CURRENT_LIMIT = 40;
   public static final IdleMode IDLE_MODE = IdleMode.kCoast;
 
-  public static final double KV = 0.0465;
+  public static final double KV = 0.074;
   public static final double KA = 0.031259;
-  public static final double KS = 0.25;
+  public static final double KS = 0.615;
 
   public static final double SHOOTER_WHEEL_GEAR_RATIO = 2.5;
   public static final double CLOSE_HUB_SHOOTER_RPM = 1085;

From adb15a11ef142b95e54ec74916c58833cb87f39c Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Sun, 12 Apr 2026 10:03:56 -0400
Subject: [PATCH 068/102] home intake logic

---
 .../subsystems/intake/IntakeConstants.java    |  1 +
 .../intake/extend/IntakeExtend.java           | 54 ++++++++++++++-----
 .../intake/extend/IntakeExtendIO.java         |  1 +
 3 files changed, 43 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 9400c56cc..fedf93f7f 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -27,6 +27,7 @@ public class IntakeConstants {
 
   // shake around the funnel pos by this much
   public static final double SHAKE_POS_OFFSET = 1;
+  public static final double HOME_VOLTAGE = 6;
   public static final double COLLAPSE_POS = 4.0;
   // How close we need to be to collapse position to stop the intake rollers
   public static final double SAFETY_TOLERANCE = 2;
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index 4b1044e1d..77f4bb8c7 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -2,6 +2,7 @@
 
 import static frc.robot.subsystems.intake.IntakeConstants.COLLAPSE_POS;
 import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POS;
+import static frc.robot.subsystems.intake.IntakeConstants.HOME_VOLTAGE;
 import static frc.robot.subsystems.intake.IntakeConstants.POSITION_TOLERANCE;
 import static frc.robot.subsystems.intake.IntakeConstants.STALL_VELOCITY;
 
@@ -25,6 +26,7 @@ public class IntakeExtend extends SubsystemBase {
   private boolean stalledCollapse = false;
   private boolean isStowed = false;
   private boolean isExtended = false;
+  private boolean hasPose = false;
 
   private double targetExtendPosition = 0;
 
@@ -54,7 +56,9 @@ public void periodic() {
     }
     if (inputs.isCollapsed) {
       io.resetExtendEncoder(0.0);
+      hasPose = true;
     }
+    inputs.hasPose = hasPose;
   }
 
   public IntakeExtend(IntakeExtendIO io, BooleanSupplier limitSwitchDisabled) {
@@ -128,26 +132,50 @@ public Command stopExtendingCommand() {
     return Commands.run(this::stopExtend, this);
   }
 
-  public Command extendToAngle(double angle) {
+  public Command homeExtend() {
     return Commands.run(
             () -> {
-              io.setPIDEnabled(true);
-              io.goToPos(angle);
+              io.setPIDEnabled(false);
+              io.setVoltageExtend(-HOME_VOLTAGE);
             },
             this)
-        .until(() -> inputs.atSetpoint)
+        .until(() -> inputs.isCollapsed)
         .finallyDo(this::stopExtend)
-        .withName("extendToAngle");
+        .withName("homeExtend");
+  }
+
+  public Command extendToAngle(double angle) {
+    Command extendCommand =
+        Commands.run(
+                () -> {
+                  io.setPIDEnabled(true);
+                  io.goToPos(angle);
+                },
+                this)
+            .until(() -> inputs.atSetpoint)
+            .finallyDo(this::stopExtend)
+            .withName("extendToAngle");
+
+    if (!hasPose) {
+      return homeExtend().andThen(extendCommand);
+    }
+    return extendCommand;
   }
 
   public Command holdAngle(double angle) {
-    return Commands.run(
-            () -> {
-              io.setPIDEnabled(true);
-              io.goToPos(angle);
-            },
-            this)
-        .finallyDo(this::stopExtend)
-        .withName("holdAngle");
+    Command holdCommand =
+        Commands.run(
+                () -> {
+                  io.setPIDEnabled(true);
+                  io.goToPos(angle);
+                },
+                this)
+            .finallyDo(this::stopExtend)
+            .withName("holdAngle");
+
+    if (!hasPose) {
+      return homeExtend().andThen(holdCommand);
+    }
+    return holdCommand;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java
index 706131aab..6a8695012 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java
@@ -21,6 +21,7 @@ class IntakeExtendIOInputs {
     public double stallExtendTimer = 0.0;
     public double stallCollapseTimer = 0.0;
     public boolean stowed = false;
+    public boolean hasPose = false;
   }
 
   default void updateInputs(IntakeExtendIOInputs inputs) {}

From f0320fd5f8064a609ce6cfe040085bdcff0e4cae Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Sun, 12 Apr 2026 10:20:33 -0400
Subject: [PATCH 069/102] update shooter gear ratio

---
 .../java/frc/robot/subsystems/shooter/ShooterConstants.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index e6873cc40..4425746f7 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -17,7 +17,7 @@ public class ShooterConstants {
   public static final double KA = 0.031259;
   public static final double KS = 0.615;
 
-  public static final double SHOOTER_WHEEL_GEAR_RATIO = 2.5;
+  public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5;
   public static final double CLOSE_HUB_SHOOTER_RPM = 1085;
 
   public static final double MAX_VOLTAGE = 12.0;

From 19fe7ed5ef446ae5b6ee7e1f2f6131eeca936348 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sun, 12 Apr 2026 09:37:25 -0500
Subject: [PATCH 070/102] saturday changes

---
 src/main/java/frc/robot/Orchestrator.java     |  2 +-
 src/main/java/frc/robot/RobotContainer.java   | 22 ++++++++-----------
 .../frc/robot/subsystems/intake/Intake.java   |  4 +---
 .../frc/robot/subsystems/shooter/Shooter.java |  2 +-
 .../subsystems/shooter/ShooterConstants.java  |  6 ++---
 5 files changed, 15 insertions(+), 21 deletions(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 5c9ed0a49..2fb2be0c7 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -51,7 +51,7 @@ public enum ZoneId {
     ZONE_4
   }
 
-  private final double FRONT_HUB_OFFSET = Units.inchesToMeters(40.0);
+  private final double FRONT_HUB_OFFSET = Units.inchesToMeters(58.0);
   private final Drive drive;
   private final Shooter shooter;
   private final MagicCarpet magicCarpet;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 00939a83a..993886d4a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -104,7 +104,7 @@ public RobotContainer() {
                   drive::addVisionMeasurement,
                   new VisionIOPhotonVision(camera0Name, robotToCamera0),
                   new VisionIOPhotonVision(camera1Name, robotToCamera1),
-                  new VisionIOPhotonVision(camera2Name, robotToCamera2));
+                new VisionIOPhotonVision(camera2Name, robotToCamera2));
           leds = new Leds();
         }
         case ROBOT_2026_COMP -> {
@@ -331,12 +331,8 @@ private void configureButtonBindings() {
                 .ignoringDisable(true));
     new Trigger(() -> driverController.getHID().getPOV() != -1)
         .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV()));
-    //    driverController.x().toggleOnTrue(orchestrator.aimToHub());
-    //
-    // driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
-    driverController.x().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(20)));
-    driverController.y().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(60)));
-    driverController.a().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(110)));
+    driverController.x().toggleOnTrue(orchestrator.aimToHub());
+    driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
     driverController
         .leftBumper()
         .and(operatorController.pov(180))
@@ -351,16 +347,16 @@ private void configureButtonBindings() {
             driverController.leftBumper(),
             () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED)
         .and(() -> !operatorController.pov(180).getAsBoolean())
-        .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS));
+        .toggleOnTrue(intake.extendToAngle(IntakeConstants.EXTEND_POS));
 
     //     VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
     driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
 
-    //    driverController
-    //        .a()
-    //        .and(operatorController.pov(180))
-    //        .onTrue(intakeExtend.resetExtendPosition());
+    driverController
+        .a()
+        .and(operatorController.pov(180))
+        .onTrue(intakeExtend.resetExtendPosition());
     driverController
         .rightBumper()
         .and(() -> !operatorController.pov(180).getAsBoolean())
@@ -370,7 +366,7 @@ private void configureButtonBindings() {
         .and(operatorController.pov(180))
         .whileTrue(intakeExtend.runIntakeExtendVolts(4))
         .onFalse(intakeExtend.stopExtendingCommand());
-    //    operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
+    operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
     operatorController
         .rightTrigger(0.1)
         .and(() -> !operatorController.pov(0).getAsBoolean())
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 468aaec25..02af35363 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -32,9 +32,7 @@ public Command holdAngleAndIntake(double angle) {
 
   public Command runIntakeMotor() {
     return rollers
-        .intake()
-        .onlyWhile(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS - SAFETY_TOLERANCE)
-        .onlyIf(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS - SAFETY_TOLERANCE);
+        .intake();
   }
 
   // Jack's Chugga Chugga mode
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 0bbe8abce..96fb9fe67 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -40,7 +40,7 @@ public class Shooter extends SubsystemBase {
 
   // Slew rate limiter: ramps the target velocity gradually (rad/s per second)
   // This prevents current spikes that cause oscillation with a 20A limit
-  private final SlewRateLimiter targetRamper = new SlewRateLimiter(1600);
+  private final SlewRateLimiter targetRamper = new SlewRateLimiter(2400);
 
   /**
    * Initializes the shooter with a Shooter IO
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 4425746f7..7cad67ea2 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -10,7 +10,7 @@ public class ShooterConstants {
   public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60;
 
   public static final double VOLTAGE_COMPENSATION = 9.0;
-  public static final int CURRENT_LIMIT = 40;
+  public static final int CURRENT_LIMIT = 20;
   public static final IdleMode IDLE_MODE = IdleMode.kCoast;
 
   public static final double KV = 0.074;
@@ -18,11 +18,11 @@ public class ShooterConstants {
   public static final double KS = 0.615;
 
   public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5;
-  public static final double CLOSE_HUB_SHOOTER_RPM = 1085;
+  public static final double CLOSE_HUB_SHOOTER_RPM = 1337;
 
   public static final double MAX_VOLTAGE = 12.0;
 
-  public static final double TOLERANCE = 50; // measured in radians
+  public static final double TOLERANCE = 5; // measured in radians
 
   public static Transform2d kShooterOffsetFromRobotCenter =
       new Transform2d(new Translation2d(-0.1839, 0.0), new Rotation2d(0.0));

From c44f31ae29ab6658f8cca97985ad65bd24866f62 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Sun, 12 Apr 2026 11:44:54 -0400
Subject: [PATCH 071/102] fix build

---
 src/main/java/frc/robot/RobotContainer.java           | 2 +-
 src/main/java/frc/robot/subsystems/intake/Intake.java | 3 +--
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 993886d4a..647244cd7 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -104,7 +104,7 @@ public RobotContainer() {
                   drive::addVisionMeasurement,
                   new VisionIOPhotonVision(camera0Name, robotToCamera0),
                   new VisionIOPhotonVision(camera1Name, robotToCamera1),
-                new VisionIOPhotonVision(camera2Name, robotToCamera2));
+                  new VisionIOPhotonVision(camera2Name, robotToCamera2));
           leds = new Leds();
         }
         case ROBOT_2026_COMP -> {
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 02af35363..750ea055b 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -31,8 +31,7 @@ public Command holdAngleAndIntake(double angle) {
   }
 
   public Command runIntakeMotor() {
-    return rollers
-        .intake();
+    return rollers.intake();
   }
 
   // Jack's Chugga Chugga mode

From 06c5d83cf068b2681166d0510fb4cdf69ddf11cd Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Sun, 12 Apr 2026 11:54:23 -0400
Subject: [PATCH 072/102] logging for intake

---
 .../frc/robot/subsystems/intake/extend/IntakeExtend.java   | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index 77f4bb8c7..32d919477 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -55,10 +55,14 @@ public void periodic() {
       RobotState.getInstance().intakePosition = IntakePosition.DEPLOYED;
     }
     if (inputs.isCollapsed) {
+      if (!hasPose) {
+        Logger.recordOutput("Intake/IntakeExtend/HomeCompleted", true);
+      }
       io.resetExtendEncoder(0.0);
       hasPose = true;
     }
     inputs.hasPose = hasPose;
+    Logger.recordOutput("Intake/IntakeExtend/HasPose", hasPose);
   }
 
   public IntakeExtend(IntakeExtendIO io, BooleanSupplier limitSwitchDisabled) {
@@ -139,6 +143,7 @@ public Command homeExtend() {
               io.setVoltageExtend(-HOME_VOLTAGE);
             },
             this)
+        .beforeStarting(() -> Logger.recordOutput("Intake/IntakeExtend/HomeStarted", true))
         .until(() -> inputs.isCollapsed)
         .finallyDo(this::stopExtend)
         .withName("homeExtend");
@@ -157,6 +162,7 @@ public Command extendToAngle(double angle) {
             .withName("extendToAngle");
 
     if (!hasPose) {
+      Logger.recordOutput("Intake/IntakeExtend/AutoHoming", true);
       return homeExtend().andThen(extendCommand);
     }
     return extendCommand;
@@ -174,6 +180,7 @@ public Command holdAngle(double angle) {
             .withName("holdAngle");
 
     if (!hasPose) {
+      Logger.recordOutput("Intake/IntakeExtend/AutoHoming", true);
       return homeExtend().andThen(holdCommand);
     }
     return holdCommand;

From 99cba051f841e0c576ea4a95dcf68a32c0d02f7e Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Sun, 12 Apr 2026 12:03:09 -0400
Subject: [PATCH 073/102] conditional command for homing intake

---
 .../subsystems/intake/extend/IntakeExtend.java      | 13 ++++---------
 1 file changed, 4 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index 32d919477..66c762559 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotState;
 import frc.robot.RobotState.IntakePosition;
@@ -150,8 +151,8 @@ public Command homeExtend() {
   }
 
   public Command extendToAngle(double angle) {
-    Command extendCommand =
-        Commands.run(
+    return
+        new ConditionalCommand(homeExtend(), Commands.none(), () -> hasPose).andThen(Commands.run(
                 () -> {
                   io.setPIDEnabled(true);
                   io.goToPos(angle);
@@ -159,13 +160,7 @@ public Command extendToAngle(double angle) {
                 this)
             .until(() -> inputs.atSetpoint)
             .finallyDo(this::stopExtend)
-            .withName("extendToAngle");
-
-    if (!hasPose) {
-      Logger.recordOutput("Intake/IntakeExtend/AutoHoming", true);
-      return homeExtend().andThen(extendCommand);
-    }
-    return extendCommand;
+            .withName("extendToAngle"));
   }
 
   public Command holdAngle(double angle) {

From 2c1d7f9a0bfedd11e445d2fc721f9da93a94ca9e Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Sun, 12 Apr 2026 12:08:36 -0400
Subject: [PATCH 074/102] fix logic

---
 .../intake/extend/IntakeExtend.java           | 21 ++++++++++---------
 1 file changed, 11 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index 66c762559..018e4a142 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -151,16 +151,17 @@ public Command homeExtend() {
   }
 
   public Command extendToAngle(double angle) {
-    return
-        new ConditionalCommand(homeExtend(), Commands.none(), () -> hasPose).andThen(Commands.run(
-                () -> {
-                  io.setPIDEnabled(true);
-                  io.goToPos(angle);
-                },
-                this)
-            .until(() -> inputs.atSetpoint)
-            .finallyDo(this::stopExtend)
-            .withName("extendToAngle"));
+    return new ConditionalCommand(homeExtend(), Commands.none(), () -> !hasPose)
+        .andThen(
+            Commands.run(
+                    () -> {
+                      io.setPIDEnabled(true);
+                      io.goToPos(angle);
+                    },
+                    this)
+                .until(() -> inputs.atSetpoint)
+                .finallyDo(this::stopExtend)
+                .withName("extendToAngle"));
   }
 
   public Command holdAngle(double angle) {

From 2ac467a92ef73b59db3f9af8481bb9a1524519b7 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Sun, 12 Apr 2026 12:11:39 -0400
Subject: [PATCH 075/102] nuke hold angle

---
 src/main/java/frc/robot/RobotContainer.java    |  2 --
 .../frc/robot/subsystems/intake/Intake.java    |  5 -----
 .../subsystems/intake/extend/IntakeExtend.java | 18 ------------------
 3 files changed, 25 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 647244cd7..c396ff0f2 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -183,8 +183,6 @@ public RobotContainer() {
         new Orchestrator(
             drive, magicCarpet, shooter, indexer, intake, intakeRollers, driverController);
     Autos autos = new Autos(drive, orchestrator, intake, intakeRollers, shooter);
-    NamedCommands.registerCommand(
-        "startIntake", intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(10.0));
     NamedCommands.registerCommand(
         "endIntake",
         Commands.parallel(
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 750ea055b..e1db70243 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -25,11 +25,6 @@ public Command extendToAngle(double angle) {
     return extend.extendToAngle(angle);
   }
 
-  public Command holdAngleAndIntake(double angle) {
-    return Commands.parallel(extend.holdAngle(angle), runIntakeMotor())
-        .withName("holdAngleAndIntake");
-  }
-
   public Command runIntakeMotor() {
     return rollers.intake();
   }
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index 018e4a142..0651b7ee7 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -163,22 +163,4 @@ public Command extendToAngle(double angle) {
                 .finallyDo(this::stopExtend)
                 .withName("extendToAngle"));
   }
-
-  public Command holdAngle(double angle) {
-    Command holdCommand =
-        Commands.run(
-                () -> {
-                  io.setPIDEnabled(true);
-                  io.goToPos(angle);
-                },
-                this)
-            .finallyDo(this::stopExtend)
-            .withName("holdAngle");
-
-    if (!hasPose) {
-      Logger.recordOutput("Intake/IntakeExtend/AutoHoming", true);
-      return homeExtend().andThen(holdCommand);
-    }
-    return holdCommand;
-  }
 }

From 1811fb774a23e263d4997ed57a3ad4cf99a39b32 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Sun, 12 Apr 2026 12:12:37 -0400
Subject: [PATCH 076/102] nuke named commands

---
 src/main/java/frc/robot/RobotContainer.java | 30 ---------------------
 1 file changed, 30 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c396ff0f2..99a042203 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -183,36 +183,6 @@ public RobotContainer() {
         new Orchestrator(
             drive, magicCarpet, shooter, indexer, intake, intakeRollers, driverController);
     Autos autos = new Autos(drive, orchestrator, intake, intakeRollers, shooter);
-    NamedCommands.registerCommand(
-        "endIntake",
-        Commands.parallel(
-                intakeRollers.stopIntakeCommand().withTimeout(0.05),
-                indexer.stop().withTimeout(0.05))
-            .withTimeout(0.05));
-    NamedCommands.registerCommand(
-        "spinUp",
-        shooter.setTargetVelocityRepeatedly(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)));
-    NamedCommands.registerCommand("feedShooter", orchestrator.feedUp());
-    NamedCommands.registerCommand("bringInIntake", intake.extendToAngleAndIntake(0));
-    NamedCommands.registerCommand("driveToHub", orchestrator.driveToHub());
-    NamedCommands.registerCommand("driveToHubAuto", orchestrator.driveToHub().withTimeout(3.0));
-    NamedCommands.registerCommand(
-        "shootAuto",
-        Commands.sequence(
-            Commands.parallel(
-                    shooter
-                        .setTargetVelocityRepeatedly(
-                            Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM))
-                        .withTimeout(0.8),
-                    intakeRollers.stopIntakeCommand(),
-                    indexer.stop())
-                .withTimeout(0.8),
-            Commands.deadline(
-                Commands.waitSeconds(3.2),
-                shooter.setTargetVelocityRepeatedly(
-                    Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)),
-                orchestrator.feedUp()),
-            Commands.parallel(indexer.stop().withTimeout(0.05)).withTimeout(0.05)));
     AutoBuilder.configure(
         drive::getPose,
         drive::setPose,

From 3be0b0343e123c655156da825c42a2d7753b6c2d Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Sun, 12 Apr 2026 12:15:16 -0400
Subject: [PATCH 077/102] fix build

---
 src/main/java/frc/robot/RobotContainer.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 99a042203..45fac5f3b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,7 +6,6 @@
 
 import static edu.wpi.first.units.Units.*;
 import static frc.robot.subsystems.drive.DriveConstants.ppConfig;
-import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM;
 import static frc.robot.subsystems.vision.VisionConstants.*;
 
 import com.pathplanner.lib.auto.AutoBuilder;

From b1e5c8714424e9eb7a42ba196a6ec7ce07cb99e3 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sun, 12 Apr 2026 15:33:21 -0500
Subject: [PATCH 078/102] sunday changes

---
 src/main/java/frc/robot/Orchestrator.java     |  3 +-
 src/main/java/frc/robot/RobotContainer.java   | 22 ++++-----
 .../frc/robot/subsystems/drive/Drive.java     |  2 +-
 .../subsystems/indexer/IndexerConstants.java  |  2 +-
 .../subsystems/indexer/IndexerIOSparkMax.java |  2 +-
 .../subsystems/intake/IntakeConstants.java    |  2 +-
 .../intake/extend/IntakeExtend.java           |  2 +-
 .../magicCarpet/MagicCarpetConstants.java     |  4 +-
 .../frc/robot/subsystems/shooter/Shooter.java | 48 +++++++++++++++++--
 .../subsystems/shooter/ShooterConstants.java  | 23 +++++----
 .../subsystems/shooter/ShooterIOSparkMax.java | 22 +++++----
 11 files changed, 90 insertions(+), 42 deletions(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 2fb2be0c7..acf8c4136 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -234,8 +234,7 @@ public Command driveToHub() {
 
   public Command feedUp() {
     return Commands.repeatingSequence(
-            Commands.parallel(magicCarpet.run(), indexer.run())
-                .until(() -> !RobotState.getInstance().shooterAtSpeed))
+            Commands.parallel(indexer.run()).until(() -> !RobotState.getInstance().shooterAtSpeed))
         .onlyIf(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0)))
         .onlyIf(() -> RobotState.getInstance().shooterAtSpeed)
         .onlyWhile(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0)))
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 45fac5f3b..5035fd99c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -9,7 +9,6 @@
 import static frc.robot.subsystems.vision.VisionConstants.*;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.pathfinding.Pathfinding;
@@ -28,7 +27,6 @@
 import frc.lib.utils.LocalADStarAK;
 import frc.robot.RobotState.IntakePosition;
 import frc.robot.commands.auto.Autos;
-import frc.robot.commands.auto.DriveToPose;
 import frc.robot.commands.drive.DriveCommands;
 import frc.robot.commands.drive.DriveWithDpad;
 import frc.robot.subsystems.drive.*;
@@ -120,7 +118,7 @@ public RobotContainer() {
                   new VisionIOPhotonVision(camera0Name, robotToCamera0),
                   new VisionIOPhotonVision(camera1Name, robotToCamera1),
                   new VisionIOPhotonVision(camera2Name, robotToCamera2));
-          leds = new Leds();
+          //          leds = new Leds();
           shooter = new Shooter(new ShooterIOSparkMax());
           magicCarpet = new MagicCarpet(new MagicCarpetSparkMax());
           indexer = new Indexer(new IndexerIOSparkMax());
@@ -202,12 +200,12 @@ public RobotContainer() {
           Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
         });
 
-    NamedCommands.registerCommand(
-        "Drive Back Left",
-        new DriveToPose(drive, () -> new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(180))));
-    NamedCommands.registerCommand(
-        "Drive Over Left",
-        new DriveToPose(drive, () -> new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(180))));
+    //    NamedCommands.registerCommand(
+    //        "Drive Back Left",
+    //        new DriveToPose(drive, () -> new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(180))));
+    //    NamedCommands.registerCommand(
+    //        "Drive Over Left",
+    //        new DriveToPose(drive, () -> new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(180))));
 
     //
     // NamedCommands.registerCommand("startShooter",Commands.parallel(orchestrator.preloadBalls(),orchestrator.prepShooter()));
@@ -276,7 +274,6 @@ public RobotContainer() {
   private void configureButtonBindings() {
     indexer.setDefaultCommand(indexer.stop());
     shooter.setDefaultCommand(shooter.stop());
-    //    shooter.setDefaultCommand(shooter.stop());
 
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(
@@ -316,7 +313,7 @@ private void configureButtonBindings() {
         .and(() -> !operatorController.pov(180).getAsBoolean())
         .toggleOnTrue(intake.extendToAngle(IntakeConstants.EXTEND_POS));
 
-    //     VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
+    //         VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
     driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
 
@@ -333,7 +330,7 @@ private void configureButtonBindings() {
         .and(operatorController.pov(180))
         .whileTrue(intakeExtend.runIntakeExtendVolts(4))
         .onFalse(intakeExtend.stopExtendingCommand());
-    operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
+    operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest());
     operatorController
         .rightTrigger(0.1)
         .and(() -> !operatorController.pov(0).getAsBoolean())
@@ -343,7 +340,6 @@ private void configureButtonBindings() {
         .rightTrigger(0.1)
         .and(operatorController.pov(0))
         .toggleOnTrue(orchestrator.spinUpShooterHub());
-    operatorController.leftTrigger(0.1).toggleOnTrue(shooter.setVoltage(4));
     operatorController.y().whileTrue(indexer.reverse());
     operatorController.x().whileTrue(intakeRollers.outtake());
   }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 09d8613f1..3add398c2 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -98,7 +98,7 @@ public Drive(
 
   @Override
   public void periodic() {
-    //    logCameraPositions(); // uncomment to show camera positions in advantage scope
+//    logCameraPositions(); // uncomment to show camera positions in advantage scope
     odometryLock.lock(); // Prevents odometry updates while reading data
     gyroIO.updateInputs(gyroInputs);
     Logger.processInputs("Drive/Gyro", gyroInputs);
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
index 2125a44b1..f32361c72 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
@@ -3,5 +3,5 @@
 public class IndexerConstants {
   public static final double ENCODER_FEEDUP_POSITION_CONVERSION = 1;
   public static final double ENCODER_FEEDUP_VELOCITY_CONVERSION = 1;
-  public static final double FEEDUP_VOLT = 12;
+  public static final double FEEDUP_VOLT = 10;
 }
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
index 4536f6fdb..c5fb5513d 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
@@ -24,7 +24,7 @@ public IndexerIOSparkMax() {
         .inverted(false)
         .idleMode(IdleMode.kBrake)
         .voltageCompensation(12)
-        .smartCurrentLimit(30);
+        .smartCurrentLimit(20);
 
     EncoderConfig feederUpEnc = new EncoderConfig();
 
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index fedf93f7f..fd09d8c8c 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -27,7 +27,7 @@ public class IntakeConstants {
 
   // shake around the funnel pos by this much
   public static final double SHAKE_POS_OFFSET = 1;
-  public static final double HOME_VOLTAGE = 6;
+  public static final double HOME_VOLTAGE = 3;
   public static final double COLLAPSE_POS = 4.0;
   // How close we need to be to collapse position to stop the intake rollers
   public static final double SAFETY_TOLERANCE = 2;
diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
index 0651b7ee7..bbf147252 100644
--- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
+++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java
@@ -141,7 +141,7 @@ public Command homeExtend() {
     return Commands.run(
             () -> {
               io.setPIDEnabled(false);
-              io.setVoltageExtend(-HOME_VOLTAGE);
+              io.setVoltageExtend(HOME_VOLTAGE);
             },
             this)
         .beforeStarting(() -> Logger.recordOutput("Intake/IntakeExtend/HomeStarted", true))
diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
index dff198558..abf58ff8b 100644
--- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
+++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
@@ -4,8 +4,8 @@ public final class MagicCarpetConstants {
 
   public static final boolean MOTOR_INVERTED = false;
 
-  public static final int CURRENT_LIMIT = 30;
-  public static final double BELT_SPEED = 0.8; // TODO: bump this value up if needed
+  public static final int CURRENT_LIMIT = 15;
+  public static final double BELT_SPEED = 0.7; // TODO: bump this value up if needed
 
   private MagicCarpetConstants() {}
   ;
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 96fb9fe67..3d6c099c8 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -3,6 +3,10 @@
 import static edu.wpi.first.units.Units.*;
 import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Volts;
+import static frc.robot.subsystems.shooter.ShooterConstants.FF_SCALAR;
+import static frc.robot.subsystems.shooter.ShooterConstants.KA;
+import static frc.robot.subsystems.shooter.ShooterConstants.KS;
+import static frc.robot.subsystems.shooter.ShooterConstants.KV;
 import static frc.robot.subsystems.shooter.ShooterConstants.TOLERANCE;
 
 import edu.wpi.first.math.controller.PIDController;
@@ -12,6 +16,7 @@
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -32,15 +37,15 @@ public class Shooter extends SubsystemBase {
   private double rampedTarget = 0.0;
 
   // Feedforward: handles steady-state voltage (V = KS + KV * velocity)
-  private final SimpleMotorFeedforward feedforward =
-      new SimpleMotorFeedforward(ShooterConstants.KS, ShooterConstants.KV);
+  private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV, KA);
   // PID: handles error correction on top of feedforward
   // P only — no I term (causes integral windup oscillation)
   private final PIDController pid = new PIDController(0.001, 0.1, 0.1, 0.02);
 
   // Slew rate limiter: ramps the target velocity gradually (rad/s per second)
   // This prevents current spikes that cause oscillation with a 20A limit
-  private final SlewRateLimiter targetRamper = new SlewRateLimiter(2400);
+  private final SlewRateLimiter targetRamper = new SlewRateLimiter(1600);
+  private double feedForwardScalar;
 
   /**
    * Initializes the shooter with a Shooter IO
@@ -48,6 +53,11 @@ public class Shooter extends SubsystemBase {
    * @param io A ShooterIO implementing instance
    */
   public Shooter(ShooterIO io) {
+    SmartDashboard.putNumber("Shooter/KS", KS * FF_SCALAR);
+    SmartDashboard.putNumber("Shooter/KA", KA * FF_SCALAR);
+    SmartDashboard.putNumber("Shooter/KV", KV * FF_SCALAR);
+    SmartDashboard.putNumber("Shooter/FeedForward_Scalar", FF_SCALAR);
+    feedForwardScalar = FF_SCALAR;
     this.io = io;
     sysId =
         new SysIdRoutine(
@@ -61,6 +71,37 @@ public Shooter(ShooterIO io) {
 
   @Override
   public void periodic() {
+
+    if (SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)
+            != feedForwardScalar
+        || SmartDashboard.getNumber(
+                    "Shooter/KS",
+                    feedforward.getKs()
+                        * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar))
+                * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)
+            != feedforward.getKs()
+        || SmartDashboard.getNumber("Shooter/KA", feedforward.getKa()) != feedforward.getKa()
+        || SmartDashboard.getNumber(
+                    "Shooter/KV",
+                    feedforward.getKv()
+                        * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar))
+                * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)
+            != feedforward.getKv()) {
+      feedForwardScalar = SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar);
+      feedforward.setKa(SmartDashboard.getNumber("Shooter/KA", feedforward.getKa()));
+      feedforward.setKv(
+          SmartDashboard.getNumber(
+                  "Shooter/KV",
+                  feedforward.getKv()
+                      * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar))
+              * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar));
+      feedforward.setKa(
+          SmartDashboard.getNumber(
+                  "Shooter/KS",
+                  feedforward.getKs()
+                      * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar))
+              * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar));
+    }
     io.updateInputs(inputs);
     RobotState.getInstance().shooterAtSpeed =
         isAtSetpoint() && targetSpeed.gt(RadiansPerSecond.of(0));
@@ -165,6 +206,7 @@ public Command setTargetVelocity(Supplier<AngularVelocity> velocity) {
               controllerEnabled = true;
             },
             this)
+        .finallyDo(io::stop)
         .withName("setTargetVelocity");
   }
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 7cad67ea2..dfdd8b6d1 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -4,25 +4,32 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
 
 public class ShooterConstants {
   public static final double ENCODER_POSITION_CONVERSION = 1;
   public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60;
 
-  public static final double VOLTAGE_COMPENSATION = 9.0;
-  public static final int CURRENT_LIMIT = 20;
+  public static final double VOLTAGE_COMPENSATION = 12.0;
+  public static final int CURRENT_LIMIT = 55;
   public static final IdleMode IDLE_MODE = IdleMode.kCoast;
 
-  public static final double KV = 0.074;
-  public static final double KA = 0.031259;
-  public static final double KS = 0.615;
+  public static final double KV = 0.0662;
+  public static final double KA = 0.028853;
+  public static final double KS = 0.2723;
 
-  public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5;
-  public static final double CLOSE_HUB_SHOOTER_RPM = 1337;
+  // for 3.7647 gear ratio
+  //  public static final double KV = 0.0662;
+  //  public static final double KA = 0.028853;
+  //  public static final double KS = 0.2723;
+  public static final double FF_SCALAR = 1;
 
+  public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5;
+  public static final double CLOSE_HUB_SHOOTER_RPM = 850;
   public static final double MAX_VOLTAGE = 12.0;
 
-  public static final double TOLERANCE = 5; // measured in radians
+  public static final double TOLERANCE =
+      Units.rotationsPerMinuteToRadiansPerSecond(100); // measured in radians
 
   public static Transform2d kShooterOffsetFromRobotCenter =
       new Transform2d(new Translation2d(-0.1839, 0.0), new Rotation2d(0.0));
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
index b019e9229..17cb653a8 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
@@ -44,26 +44,26 @@ public ShooterIOSparkMax(boolean independentMode) {
         .inverted(false)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
-        .smartCurrentLimit(CURRENT_LIMIT);
-    bottomLeftMotorConfig.follow(shooterTopRightMotorCanId, false);
+        .smartCurrentLimit(CURRENT_LIMIT)
+        .secondaryCurrentLimit(CURRENT_LIMIT);
     bottomLeftMotorConfig.apply(enc);
 
     var topLeftMotorConfig = new SparkMaxConfig();
     topLeftMotorConfig
-        .inverted(false)
+        .inverted(true)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
-        .smartCurrentLimit(CURRENT_LIMIT);
-    topLeftMotorConfig.follow(shooterTopRightMotorCanId, true);
+        .smartCurrentLimit(CURRENT_LIMIT)
+        .secondaryCurrentLimit(CURRENT_LIMIT);
     topLeftMotorConfig.apply(enc);
 
     var bottomRightMotorConfig = new SparkMaxConfig();
     bottomRightMotorConfig
-        .inverted(false)
+        .inverted(true)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
-        .smartCurrentLimit(CURRENT_LIMIT);
-    bottomRightMotorConfig.follow(shooterTopRightMotorCanId, true);
+        .smartCurrentLimit(CURRENT_LIMIT)
+        .secondaryCurrentLimit(CURRENT_LIMIT);
     bottomRightMotorConfig.apply(enc);
 
     var topRightMotorConfig = new SparkMaxConfig();
@@ -71,7 +71,8 @@ public ShooterIOSparkMax(boolean independentMode) {
         .inverted(false)
         .idleMode(IDLE_MODE)
         .voltageCompensation(VOLTAGE_COMPENSATION)
-        .smartCurrentLimit(CURRENT_LIMIT);
+        .smartCurrentLimit(CURRENT_LIMIT)
+        .secondaryCurrentLimit(CURRENT_LIMIT);
     topRightMotorConfig.apply(enc);
 
     bottomLeftMotor.configure(
@@ -125,6 +126,9 @@ public void updateInputs(ShooterIOInputs inputs) {
   @Override
   public void setVoltage(double volts) {
     topRightMotor.setVoltage(volts);
+    topLeftMotor.setVoltage(volts);
+    bottomRightMotor.setVoltage(volts);
+    bottomLeftMotor.setVoltage(volts);
   }
 
   @Override

From 869708902fc237f90f2bc7725dcb7af04ad14843 Mon Sep 17 00:00:00 2001
From: Ronit Sharma <rshar272k@gmail.com>
Date: Sun, 12 Apr 2026 16:54:12 -0400
Subject: [PATCH 079/102] Adding controller rumble feature when intaking

---
 src/main/java/frc/robot/RobotContainer.java | 21 ++++++++++++++++++++-
 1 file changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5035fd99c..d6b8b2de5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -17,8 +17,10 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -314,8 +316,17 @@ private void configureButtonBindings() {
         .toggleOnTrue(intake.extendToAngle(IntakeConstants.EXTEND_POS));
 
     //         VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
-    driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
+    // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
+  
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
+    driverController.leftTrigger(0.2).toggleOnTrue(
+    Commands.parallel(
+        rumblePulse(0.5, 0.2),
+        intake.runIntakeMotor()
+    ).finallyDo(() ->
+        CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15))
+    )
+);
 
     driverController
         .a()
@@ -357,4 +368,12 @@ public void robotPeriodic() {
     RobotState.getInstance().updateLEDState();
     orchestrator.orchestratorPeriodic();
   }
+
+  private Command rumblePulse(double intensity, double seconds) {
+    return Commands.sequence(
+        Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)),
+        Commands.waitSeconds(seconds),
+        Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0))
+    );
+}
 }

From b842701218bbca8b6f16a1633866b5e5d47af0f7 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sun, 12 Apr 2026 17:11:33 -0400
Subject: [PATCH 080/102] autos to go under tench + removes named cmmands

---
 .../deploy/pathplanner/paths/A-Cycle1.path    | 107 ++++++--------
 .../deploy/pathplanner/paths/A-Cycle2.path    |  60 ++++----
 .../deploy/pathplanner/paths/B-Cycle1.path    | 133 ++++++++----------
 .../deploy/pathplanner/paths/B-Cycle2.path    |  96 ++++++-------
 4 files changed, 175 insertions(+), 221 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index 9d22515a8..680284a2a 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.220865926213596,
-        "y": 7.587085598717599
+        "x": 5.365636024664715,
+        "y": 7.580018615477625
       },
       "isLocked": false,
       "linkedName": null
@@ -20,56 +20,72 @@
         "y": 6.832881932021468
       },
       "prevControl": {
-        "x": 7.335202354903501,
-        "y": 7.21155347245828
+        "x": 7.352205583793333,
+        "y": 7.527391495153063
       },
       "nextControl": {
-        "x": 7.834164231584512,
-        "y": 6.465719538012889
+        "x": 7.96399098844635,
+        "y": 5.729501726483708
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.7021109123434695,
-        "y": 4.074563506261182
+        "x": 7.896815742397138,
+        "y": 4.0593381037567084
       },
       "prevControl": {
-        "x": 8.22271615942871,
-        "y": 4.362336125245133
+        "x": 7.990620981847936,
+        "y": 4.332081885610835
       },
       "nextControl": {
-        "x": 7.200064179283715,
-        "y": 3.7970493935359855
+        "x": 7.809668414958673,
+        "y": 3.8059525593755392
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.965992844364937,
-        "y": 5.664652951699464
+        "x": 5.463005366726297,
+        "y": 7.369320214669052
       },
       "prevControl": {
-        "x": 6.215916040758477,
-        "y": 5.658456508813674
+        "x": 7.167489045136595,
+        "y": 6.930742523290983
       },
       "nextControl": {
-        "x": 4.002719141323792,
-        "y": 5.713329159212882
+        "x": 4.218132652871465,
+        "y": 7.689636218757288
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.256,
-        "y": 5.665
+        "x": 3.402379248658318,
+        "y": 7.369320214669052
       },
       "prevControl": {
-        "x": 4.375552524919555,
-        "y": 5.729905885962339
+        "x": 3.755531829449267,
+        "y": 7.579788697952738
+      },
+      "nextControl": {
+        "x": 2.276559141756934,
+        "y": 6.698364760176492
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.256350626118068,
+        "y": 4.553276315789475
+      },
+      "prevControl": {
+        "x": 2.412531719157826,
+        "y": 5.065468189438994
       },
       "nextControl": null,
       "isLocked": false,
@@ -87,7 +103,7 @@
     },
     {
       "waypointRelativePos": 1.9411764705882377,
-      "rotationDegrees": -136.0
+      "rotationDegrees": -106.0
     },
     {
       "waypointRelativePos": 3.0160427807486627,
@@ -101,7 +117,7 @@
   "constraintZones": [
     {
       "name": "Constraints Zone",
-      "minWaypointRelativePos": 1.1254237288135598,
+      "minWaypointRelativePos": 1.0508474576271185,
       "maxWaypointRelativePos": 2.1694915254237293,
       "constraints": {
         "maxVelocity": 1.2,
@@ -111,49 +127,10 @@
         "nominalVoltage": 12,
         "unlimited": false
       }
-    },
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 3.118644067796608,
-      "maxWaypointRelativePos": 3.5254237288135593,
-      "constraints": {
-        "maxVelocity": 1.2,
-        "maxAcceleration": 6,
-        "maxAngularVelocity": 540,
-        "maxAngularAcceleration": 720,
-        "nominalVoltage": 12,
-        "unlimited": false
-      }
-    },
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 0,
-      "maxWaypointRelativePos": 0,
-      "constraints": {
-        "maxVelocity": 4,
-        "maxAcceleration": 6,
-        "maxAngularVelocity": 540,
-        "maxAngularAcceleration": 720,
-        "nominalVoltage": 12,
-        "unlimited": false
-      }
     }
   ],
   "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "startIntake",
-      "waypointRelativePos": 0.5288135593220339,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "preloadBalls",
-      "waypointRelativePos": 2.535593220338976,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
+  "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
     "maxAcceleration": 6.0,
@@ -164,7 +141,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -51.09865544424842
+    "rotation": -23.4357656437963
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index 976a35738..4532e53ee 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -4,12 +4,12 @@
     {
       "anchor": {
         "x": 3.256350626118068,
-        "y": 5.664652951699464
+        "y": 4.553276315789475
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.6674363972882924,
-        "y": 5.6284350881968495
+        "x": 3.073045025903002,
+        "y": 5.010824871550651
       },
       "isLocked": false,
       "linkedName": "A-Cycle1-End"
@@ -20,12 +20,12 @@
         "y": 7.254742397137747
       },
       "prevControl": {
-        "x": 2.1979715994142452,
-        "y": 6.347572517197855
+        "x": 1.7727566660512677,
+        "y": 6.028530039315472
       },
       "nextControl": {
-        "x": 3.5736065913462336,
-        "y": 7.526672441691426
+        "x": 3.449050576638838,
+        "y": 7.414011757246001
       },
       "isLocked": false,
       "linkedName": null
@@ -36,12 +36,12 @@
         "y": 7.254742397137747
       },
       "prevControl": {
-        "x": 5.8071316550071765,
-        "y": 7.778212679002352
+        "x": 5.771047211855309,
+        "y": 7.83231450077766
       },
       "nextControl": {
-        "x": 7.57757028119644,
-        "y": 6.342250134115599
+        "x": 7.749507558733082,
+        "y": 6.1555316523359425
       },
       "isLocked": false,
       "linkedName": null
@@ -52,12 +52,12 @@
         "y": 4.409032894736844
       },
       "prevControl": {
-        "x": 6.863907746462161,
-        "y": 5.216361806741775
+        "x": 6.559973370874717,
+        "y": 4.568129553133817
       },
       "nextControl": {
-        "x": 5.937085933334687,
-        "y": 3.8213770349498226
+        "x": 5.871274488423377,
+        "y": 4.096763618189595
       },
       "isLocked": false,
       "linkedName": null
@@ -68,12 +68,12 @@
         "y": 4.553276315789475
       },
       "prevControl": {
-        "x": 5.799882977639703,
-        "y": 4.304117085888362
+        "x": 5.751215757710489,
+        "y": 4.286409263322725
       },
       "nextControl": {
-        "x": 5.6990813534677445,
-        "y": 5.5301053915928176
+        "x": 5.805650962114751,
+        "y": 4.801893945471092
       },
       "isLocked": false,
       "linkedName": null
@@ -84,12 +84,12 @@
         "y": 7.447760964912282
       },
       "prevControl": {
-        "x": 6.523025544615784,
-        "y": 6.993212709981212
+        "x": 6.769357739292765,
+        "y": 6.851722923583836
       },
       "nextControl": {
-        "x": 5.527831315774364,
-        "y": 7.601532227331069
+        "x": 5.565220720039658,
+        "y": 7.576712704575512
       },
       "isLocked": false,
       "linkedName": null
@@ -100,12 +100,12 @@
         "y": 7.447760964912282
       },
       "prevControl": {
-        "x": 3.619467665950238,
-        "y": 7.613424526324748
+        "x": 3.7599789090650377,
+        "y": 7.631925045537918
       },
       "nextControl": {
-        "x": 2.9084995834559355,
-        "y": 7.3654202472362
+        "x": 2.9050441758844623,
+        "y": 7.3760898401707475
       },
       "isLocked": false,
       "linkedName": null
@@ -116,8 +116,8 @@
         "y": 6.284197368421054
       },
       "prevControl": {
-        "x": 1.9265215510296607,
-        "y": 7.001825662162521
+        "x": 1.867329032576315,
+        "y": 6.919804106628433
       },
       "nextControl": null,
       "isLocked": false,
@@ -187,7 +187,7 @@
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -51.09865544424842
+    "rotation": -23.4357656437963
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index 14317bd18..036b0753e 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -4,12 +4,12 @@
     {
       "anchor": {
         "x": 4.295,
-        "y": 0.538
+        "y": 0.5380000000000011
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.220865926213596,
-        "y": 0.4819144012824006
+        "x": 5.260984348859008,
+        "y": 0.548709162061195
       },
       "isLocked": false,
       "linkedName": null
@@ -17,63 +17,79 @@
     {
       "anchor": {
         "x": 7.588533094812165,
-        "y": 1.236118067978532
+        "y": 1.2361180679785324
       },
       "prevControl": {
-        "x": 7.335202354903501,
-        "y": 0.8574465275417207
+        "x": 7.333281138772382,
+        "y": 0.5372671705796467
       },
       "nextControl": {
-        "x": 7.834164231584512,
-        "y": 1.603280461987111
+        "x": 8.01198853278679,
+        "y": 2.3954910162934935
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.7021109123434695,
-        "y": 3.994436493738819
+        "x": 7.896815742397138,
+        "y": 4.009661896243292
       },
       "prevControl": {
-        "x": 8.22271615942871,
-        "y": 3.7066638747548675
+        "x": 7.956228852277296,
+        "y": 3.7035208327957765
       },
       "nextControl": {
-        "x": 7.200064179283715,
-        "y": 4.271950606464015
+        "x": 7.8491866370218535,
+        "y": 4.255082898443899
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.965992844364937,
-        "y": 2.4043470483005365
+        "x": 5.463005366726297,
+        "y": 0.6996797853309484
       },
       "prevControl": {
-        "x": 6.215916040758477,
-        "y": 2.410543491186327
+        "x": 7.033869255673328,
+        "y": 1.1479768709205327
       },
       "nextControl": {
-        "x": 4.002719141323792,
-        "y": 2.355670840787119
+        "x": 4.179018869195575,
+        "y": 0.33325249087882547
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.175,
-        "y": 2.404
+        "x": 3.402379248658318,
+        "y": 0.6996797853309484
       },
       "prevControl": {
-        "x": 4.294552524919553,
-        "y": 2.3390941140376613
+        "x": 3.6989618406989995,
+        "y": 0.5442676850187853
+      },
+      "nextControl": {
+        "x": 2.3915233528638398,
+        "y": 1.2293778933601753
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.4510554561717353,
+        "y": 3.2642933810375667
+      },
+      "prevControl": {
+        "x": 2.5378407958797427,
+        "y": 2.6730525818495314
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "B-Cycle1-End"
+      "linkedName": "B-Cycle-End"
     }
   ],
   "rotationTargets": [
@@ -83,11 +99,11 @@
     },
     {
       "waypointRelativePos": 1.171122994652407,
-      "rotationDegrees": 86.315500893566
+      "rotationDegrees": 86.315
     },
     {
       "waypointRelativePos": 1.9411764705882377,
-      "rotationDegrees": 115.5063543100694
+      "rotationDegrees": 94.0
     },
     {
       "waypointRelativePos": 3.0160427807486627,
@@ -101,70 +117,31 @@
   "constraintZones": [
     {
       "name": "Constraints Zone",
-      "minWaypointRelativePos": 1.1254237288135598,
+      "minWaypointRelativePos": 1.0508474576271185,
       "maxWaypointRelativePos": 2.1694915254237293,
       "constraints": {
         "maxVelocity": 1.2,
-        "maxAcceleration": 6.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    },
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 3.118644067796608,
-      "maxWaypointRelativePos": 3.5254237288135593,
-      "constraints": {
-        "maxVelocity": 1.2,
-        "maxAcceleration": 6.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
-        "unlimited": false
-      }
-    },
-    {
-      "name": "Constraints Zone",
-      "minWaypointRelativePos": 0,
-      "maxWaypointRelativePos": 0,
-      "constraints": {
-        "maxVelocity": 4.0,
-        "maxAcceleration": 6.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
+        "maxAcceleration": 6,
+        "maxAngularVelocity": 540,
+        "maxAngularAcceleration": 720,
+        "nominalVoltage": 12,
         "unlimited": false
       }
     }
   ],
   "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "startIntake",
-      "waypointRelativePos": 0.5288135593220339,
-      "endWaypointRelativePos": null,
-      "command": null
-    },
-    {
-      "name": "preloadBalls",
-      "waypointRelativePos": 2.535593220338976,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
+  "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "maxVelocity": 4,
+    "maxAcceleration": 6,
+    "maxAngularVelocity": 540,
+    "maxAngularAcceleration": 720,
+    "nominalVoltage": 12,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 51.95295746817396
+    "rotation": 37.71655912222805
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path
index 5ddf30eec..ee6cc4b5c 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path
@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.1752236135957066,
-        "y": 2.4043470483005365
+        "x": 3.4510554561717353,
+        "y": 3.2642933810375667
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.586309384765931,
-        "y": 2.4405649118031514
+        "x": 3.2421167389384307,
+        "y": 2.967186063127149
       },
       "isLocked": false,
-      "linkedName": "B-Cycle1-End"
+      "linkedName": "B-Cycle-End"
     },
     {
       "anchor": {
         "x": 3.256350626118068,
-        "y": 0.8142576028622535
+        "y": 0.8142576028622539
       },
       "prevControl": {
-        "x": 2.1979715994142452,
-        "y": 1.721427482802146
+        "x": 1.5889035900296817,
+        "y": 2.0483193166651414
       },
       "nextControl": {
-        "x": 3.5736065913462336,
-        "y": 0.5423275583085745
+        "x": 3.457302404516393,
+        "y": 0.6655351003330456
       },
       "isLocked": false,
       "linkedName": null
@@ -33,15 +33,15 @@
     {
       "anchor": {
         "x": 6.4525328947368426,
-        "y": 0.8142576028622535
+        "y": 0.8142576028622539
       },
       "prevControl": {
-        "x": 5.8071316550071765,
-        "y": 0.2907873209976489
+        "x": 5.752347562091977,
+        "y": 0.24039328263318294
       },
       "nextControl": {
-        "x": 7.57757028119644,
-        "y": 1.7267498658844018
+        "x": 7.910712691624192,
+        "y": 2.0093659818847356
       },
       "isLocked": false,
       "linkedName": null
@@ -52,12 +52,12 @@
         "y": 3.6599671052631573
       },
       "prevControl": {
-        "x": 6.863907746462161,
-        "y": 2.852638193258226
+        "x": 6.59100273960581,
+        "y": 3.4940005006714894
       },
       "nextControl": {
-        "x": 5.937085933334687,
-        "y": 4.247622965050178
+        "x": 5.920916329346852,
+        "y": 3.9160880386065315
       },
       "isLocked": false,
       "linkedName": null
@@ -68,12 +68,12 @@
         "y": 3.515723684210526
       },
       "prevControl": {
-        "x": 5.799882977639703,
-        "y": 3.764882914111639
+        "x": 5.837373947319441,
+        "y": 3.7589081110194775
       },
       "nextControl": {
-        "x": 5.6990813534677445,
-        "y": 2.538894608407183
+        "x": 5.6945270228567875,
+        "y": 3.159737112788334
       },
       "isLocked": false,
       "linkedName": null
@@ -81,15 +81,15 @@
     {
       "anchor": {
         "x": 5.779396929824561,
-        "y": 0.6212390350877184
+        "y": 0.6212390350877186
       },
       "prevControl": {
-        "x": 6.523025544615784,
-        "y": 1.0757872900187888
+        "x": 6.736601761736383,
+        "y": 1.2079600435401154
       },
       "nextControl": {
-        "x": 5.527831315774364,
-        "y": 0.46746777266893197
+        "x": 5.5662512875867165,
+        "y": 0.49059090002633776
       },
       "isLocked": false,
       "linkedName": null
@@ -97,15 +97,15 @@
     {
       "anchor": {
         "x": 3.1445504385964913,
-        "y": 0.6212390350877184
+        "y": 0.6212390350877186
       },
       "prevControl": {
-        "x": 3.619467665950238,
-        "y": 0.4555754736752522
+        "x": 3.5589239547141958,
+        "y": 0.44627660534504465
       },
       "nextControl": {
-        "x": 2.9084995834559355,
-        "y": 0.7035797527638007
+        "x": 2.8278583396709256,
+        "y": 0.7549570864425861
       },
       "isLocked": false,
       "linkedName": null
@@ -113,11 +113,11 @@
     {
       "anchor": {
         "x": 2.548344298245614,
-        "y": 1.784802631578947
+        "y": 1.7848026315789465
       },
       "prevControl": {
-        "x": 1.9265215510296607,
-        "y": 1.0671743378374803
+        "x": 1.9675297251591926,
+        "y": 1.3628954494520977
       },
       "nextControl": null,
       "isLocked": false,
@@ -142,7 +142,7 @@
       "rotationDegrees": 119.99999999999999
     },
     {
-      "waypointRelativePos": 3.9622641509434033,
+      "waypointRelativePos": 3.962264150943403,
       "rotationDegrees": -100.0
     },
     {
@@ -161,10 +161,10 @@
       "maxWaypointRelativePos": 4.235188509874326,
       "constraints": {
         "maxVelocity": 1.5,
-        "maxAcceleration": 6.0,
-        "maxAngularVelocity": 540.0,
-        "maxAngularAcceleration": 720.0,
-        "nominalVoltage": 12.0,
+        "maxAcceleration": 6,
+        "maxAngularVelocity": 540,
+        "maxAngularAcceleration": 720,
+        "nominalVoltage": 12,
         "unlimited": false
       }
     }
@@ -172,22 +172,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "maxVelocity": 4,
+    "maxAcceleration": 6,
+    "maxAngularVelocity": 540,
+    "maxAngularAcceleration": 720,
+    "nominalVoltage": 12,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 55.84030545433046
+    "rotation": 38.90134455575158
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 51.95295746817396
+    "rotation": 37.71655912222805
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file

From a7fc35387d1a1939becd1db8a2e35c3cbbf08bc9 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sun, 12 Apr 2026 17:15:03 -0400
Subject: [PATCH 081/102] Fixed autos to go iunder trench + get rid of named
 commands

bla bla bla
---
 .../pathplanner/autos/A-Side 2 Cycle.auto     | 25 +++++++++++++++++++
 .../pathplanner/autos/B-Side 2 Cycle.auto     | 25 +++++++++++++++++++
 2 files changed, 50 insertions(+)
 create mode 100644 src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto
 create mode 100644 src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto

diff --git a/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto b/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto
new file mode 100644
index 000000000..ce72f9f46
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto	
@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "A-Cycle1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "A-Cycle2"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto b/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto
new file mode 100644
index 000000000..6961e215d
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto	
@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "B-Cycle1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "B-Cycle2"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file

From a82c8e943056c38504c69f0ba72c853f1cdf4e2e Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sun, 12 Apr 2026 17:19:43 -0400
Subject: [PATCH 082/102] bla bla bla

---
 src/main/java/frc/robot/RobotContainer.java   | 23 ++++++++-----------
 .../frc/robot/subsystems/drive/Drive.java     |  2 +-
 2 files changed, 11 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d6b8b2de5..1bf04af26 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -317,16 +317,13 @@ private void configureButtonBindings() {
 
     //         VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
     // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
-  
+
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
-    driverController.leftTrigger(0.2).toggleOnTrue(
-    Commands.parallel(
-        rumblePulse(0.5, 0.2),
-        intake.runIntakeMotor()
-    ).finallyDo(() ->
-        CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15))
-    )
-);
+    driverController
+        .leftTrigger(0.2)
+        .toggleOnTrue(
+            Commands.parallel(rumblePulse(0.5, 0.2), intake.runIntakeMotor())
+                .finallyDo(() -> CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15))));
 
     driverController
         .a()
@@ -371,9 +368,9 @@ public void robotPeriodic() {
 
   private Command rumblePulse(double intensity, double seconds) {
     return Commands.sequence(
-        Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)),
+        Commands.runOnce(
+            () -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)),
         Commands.waitSeconds(seconds),
-        Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0))
-    );
-}
+        Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0)));
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 3add398c2..09d8613f1 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -98,7 +98,7 @@ public Drive(
 
   @Override
   public void periodic() {
-//    logCameraPositions(); // uncomment to show camera positions in advantage scope
+    //    logCameraPositions(); // uncomment to show camera positions in advantage scope
     odometryLock.lock(); // Prevents odometry updates while reading data
     gyroIO.updateInputs(gyroInputs);
     Logger.processInputs("Drive/Gyro", gyroInputs);

From 64566f5d6ceaedc39a2fb85ad6d69ddb4210426a Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sun, 12 Apr 2026 18:02:02 -0400
Subject: [PATCH 083/102] fix autos and auto logic

---
 .../deploy/pathplanner/paths/A-Cycle1.path    | 10 ++--
 .../deploy/pathplanner/paths/B-Cycle1.path    | 10 ++--
 .../java/frc/robot/commands/auto/Autos.java   | 51 ++++++++++++++-----
 3 files changed, 48 insertions(+), 23 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index 680284a2a..c105af3f0 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -80,12 +80,12 @@
     },
     {
       "anchor": {
-        "x": 3.256350626118068,
-        "y": 4.553276315789475
+        "x": 3.256,
+        "y": 4.553
       },
       "prevControl": {
-        "x": 2.412531719157826,
-        "y": 5.065468189438994
+        "x": 2.412181093039758,
+        "y": 5.065191873649519
       },
       "nextControl": null,
       "isLocked": false,
@@ -141,7 +141,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -23.4357656437963
+    "rotation": -23.436
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index 036b0753e..23d68c24c 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -80,12 +80,12 @@
     },
     {
       "anchor": {
-        "x": 3.4510554561717353,
-        "y": 3.2642933810375667
+        "x": 3.451,
+        "y": 3.264
       },
       "prevControl": {
-        "x": 2.5378407958797427,
-        "y": 2.6730525818495314
+        "x": 2.5377853397080075,
+        "y": 2.6727592008119645
       },
       "nextControl": null,
       "isLocked": false,
@@ -141,7 +141,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 37.71655912222805
+    "rotation": 37.717000000000006
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index c145ea0e5..b727ac44a 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -98,14 +98,6 @@ public Autos(
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000)));
   private static final Supplier<Pose2d> startBside =
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000)));
-  private static final Supplier<Pose2d> connectBside =
-      () ->
-          AllianceFlipUtil.apply(
-              new Pose2d(3.175, 2.404, new Rotation2d(Units.degreesToRadians(-118.29 + 180))));
-  private static final Supplier<Pose2d> connectAside =
-      () ->
-          AllianceFlipUtil.apply(
-              new Pose2d(3.256, 5.665, new Rotation2d(Units.degreesToRadians(115.83 + 180))));
 
   /**
    * Helper function for a single cycle auto with a constant shooting speed
@@ -130,6 +122,21 @@ private Command ppCycle(String path, Supplier<Pose2d> startPose) {
             orchestrator.feedUp()));
   }
 
+  private Command ppCycleConnect(String path) {
+    return Commands.sequence(
+        Commands.deadline(
+                drive.getAutonomousCommand(path),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
+                orchestrator.spinUpShooterHub())
+            .withTimeout(14.5),
+        orchestrator.aimToHub().withTimeout(2.5),
+        Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5),
+        Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooter(1214),
+            orchestrator.feedUp()));
+  }
+
   /**
    * Helper function for a single cycle auto that uses shooter regression to shoot into the hub
    *
@@ -157,6 +164,25 @@ private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
             orchestrator.feedUp()));
   }
 
+  private Command ppCycleRegressionConnect(String path) {
+    return Commands.sequence(
+        Commands.deadline(
+                drive.getAutonomousCommand(path),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+            .withTimeout(14.5),
+        Commands.deadline(
+            orchestrator.aimToHub().withTimeout(2.5),
+            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
+        Commands.parallel(
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                orchestrator.feedUp())
+            .withTimeout(2.5),
+        Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+            orchestrator.feedUp()));
+  }
+
   /**
    * Helper function for a double cycle auto that uses shooter regression to shoot into the hub
    * twice (once after each cycle)
@@ -167,11 +193,10 @@ private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
    *     up in after the path
    * @return A command that follows the paths and shoots
    */
-  private Command pp2CycleRegression(
-      String path1, String path2, Supplier<Pose2d> startPose, Supplier<Pose2d> startPose2) {
+  private Command pp2CycleRegression(String path1, String path2, Supplier<Pose2d> startPose) {
     return ppCycleRegression(path1, startPose)
         .withTimeout(10.5)
-        .andThen(ppCycleRegression(path2, startPose2));
+        .andThen(ppCycleRegressionConnect(path2));
   }
 
   /**
@@ -198,7 +223,7 @@ public Command ppBCycleRight() {
    * @return A command that executes the auto
    */
   public Command ppB2CycleRightRegression() {
-    return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside, connectBside);
+    return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside);
   }
 
   /**
@@ -207,7 +232,7 @@ public Command ppB2CycleRightRegression() {
    * @return A comand that executes the auto
    */
   public Command ppA2CycleRightRegression() {
-    return pp2CycleRegression("A-Cycle1", "A-Cycle2", startAside, connectAside);
+    return pp2CycleRegression("A-Cycle1", "A-Cycle2", startAside);
   }
 
   /**

From 47818c3f4e4f35afc0bf2589cccd39390c5355e8 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sun, 12 Apr 2026 18:21:32 -0500
Subject: [PATCH 084/102] v2 working with regression and auto aim

---
 src/main/java/frc/robot/Orchestrator.java     | 15 ++++++++++--
 src/main/java/frc/robot/RobotContainer.java   | 23 ++++++++-----------
 .../frc/robot/commands/auto/DriveToPose.java  |  2 +-
 .../frc/robot/subsystems/drive/Drive.java     |  2 +-
 .../subsystems/indexer/IndexerConstants.java  |  2 +-
 .../subsystems/indexer/IndexerIOSparkMax.java |  2 +-
 .../magicCarpet/MagicCarpetConstants.java     |  4 ++--
 .../frc/robot/subsystems/shooter/Shooter.java |  8 +++----
 .../subsystems/shooter/ShooterConstants.java  |  2 +-
 9 files changed, 34 insertions(+), 26 deletions(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index acf8c4136..93d7bb2c2 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -29,6 +30,7 @@
 import frc.robot.subsystems.intake.rollers.IntakeRollers;
 import frc.robot.subsystems.magicCarpet.MagicCarpet;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.util.ShooterLeadCompensator;
 import frc.robot.util.Zone;
 import frc.robot.util.Zone.Tuple2d;
@@ -186,7 +188,11 @@ public Supplier<Distance> getHubDistance() {
     return () ->
         Meters.of(
             AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d())
-                .getDistance(drive.getPose().getTranslation()));
+                .getDistance(
+                    drive
+                        .getPose()
+                        .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter)
+                        .getTranslation()));
   }
 
   private Rotation2d filteredHubAngle(Rotation2d raw) {
@@ -291,7 +297,12 @@ public Command aimToHub() {
                 drive.getPose().getX(),
                 drive.getPose().getY(),
                 AllianceFlipUtil.apply(Hub.blueCenter)
-                    .minus(drive.getPose().getTranslation())
+                    .plus(new Translation2d(-0.6, 0))
+                    .minus(
+                        drive
+                            .getPose()
+                            .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter)
+                            .getTranslation())
                     .getAngle()));
   }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d6b8b2de5..1bf04af26 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -317,16 +317,13 @@ private void configureButtonBindings() {
 
     //         VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE
     // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
-  
+
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
-    driverController.leftTrigger(0.2).toggleOnTrue(
-    Commands.parallel(
-        rumblePulse(0.5, 0.2),
-        intake.runIntakeMotor()
-    ).finallyDo(() ->
-        CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15))
-    )
-);
+    driverController
+        .leftTrigger(0.2)
+        .toggleOnTrue(
+            Commands.parallel(rumblePulse(0.5, 0.2), intake.runIntakeMotor())
+                .finallyDo(() -> CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15))));
 
     driverController
         .a()
@@ -371,9 +368,9 @@ public void robotPeriodic() {
 
   private Command rumblePulse(double intensity, double seconds) {
     return Commands.sequence(
-        Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)),
+        Commands.runOnce(
+            () -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)),
         Commands.waitSeconds(seconds),
-        Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0))
-    );
-}
+        Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0)));
+  }
 }
diff --git a/src/main/java/frc/robot/commands/auto/DriveToPose.java b/src/main/java/frc/robot/commands/auto/DriveToPose.java
index 9f51e5912..4b9eafb20 100644
--- a/src/main/java/frc/robot/commands/auto/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/auto/DriveToPose.java
@@ -64,7 +64,7 @@ public class DriveToPose extends Command {
         thetaMaxVelocity.initDefault(Units.degreesToRadians(360.0));
         thetaMaxAcceleration.initDefault(Units.degreesToRadians(720.0));
         driveTolerance.initDefault(0.01);
-        thetaTolerance.initDefault(Units.degreesToRadians(1.0));
+        thetaTolerance.initDefault(Units.degreesToRadians(0.2));
       case ROBOT_2025_COMP:
         driveKp.initDefault(2.5);
         driveKd.initDefault(0.0);
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 3add398c2..09d8613f1 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -98,7 +98,7 @@ public Drive(
 
   @Override
   public void periodic() {
-//    logCameraPositions(); // uncomment to show camera positions in advantage scope
+    //    logCameraPositions(); // uncomment to show camera positions in advantage scope
     odometryLock.lock(); // Prevents odometry updates while reading data
     gyroIO.updateInputs(gyroInputs);
     Logger.processInputs("Drive/Gyro", gyroInputs);
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
index f32361c72..2125a44b1 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
@@ -3,5 +3,5 @@
 public class IndexerConstants {
   public static final double ENCODER_FEEDUP_POSITION_CONVERSION = 1;
   public static final double ENCODER_FEEDUP_VELOCITY_CONVERSION = 1;
-  public static final double FEEDUP_VOLT = 10;
+  public static final double FEEDUP_VOLT = 12;
 }
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
index c5fb5513d..4536f6fdb 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java
@@ -24,7 +24,7 @@ public IndexerIOSparkMax() {
         .inverted(false)
         .idleMode(IdleMode.kBrake)
         .voltageCompensation(12)
-        .smartCurrentLimit(20);
+        .smartCurrentLimit(30);
 
     EncoderConfig feederUpEnc = new EncoderConfig();
 
diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
index abf58ff8b..8d715dfe1 100644
--- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
+++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java
@@ -4,8 +4,8 @@ public final class MagicCarpetConstants {
 
   public static final boolean MOTOR_INVERTED = false;
 
-  public static final int CURRENT_LIMIT = 15;
-  public static final double BELT_SPEED = 0.7; // TODO: bump this value up if needed
+  public static final int CURRENT_LIMIT = 20;
+  public static final double BELT_SPEED = 0.8; // TODO: bump this value up if needed
 
   private MagicCarpetConstants() {}
   ;
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 3d6c099c8..569fd178f 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -261,11 +261,11 @@ public Time getAirTimeSeconds(Distance distance) {
    * @return Setpoint in radians per second
    */
   public Supplier<AngularVelocity> calculateSetpoint(Supplier<Distance> distance) {
-    // calculate rad/s depending on distance
+    // calculate rpm depending on distance
     return () -> {
-      AngularVelocity velocity = RadiansPerSecond.of(183.35 * distance.get().in(Meters) + 750.93);
-      if (velocity.gt(RadiansPerSecond.of(5000))) {
-        return RadiansPerSecond.of(5000);
+      AngularVelocity velocity = RPM.of(102.4367 * distance.get().in(Meters) + 799.60799);
+      if (velocity.gt(RPM.of(1550))) {
+        return RPM.of(1550);
       } else return velocity;
     };
   }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index dfdd8b6d1..e2e0cd5f3 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -32,5 +32,5 @@ public class ShooterConstants {
       Units.rotationsPerMinuteToRadiansPerSecond(100); // measured in radians
 
   public static Transform2d kShooterOffsetFromRobotCenter =
-      new Transform2d(new Translation2d(-0.1839, 0.0), new Rotation2d(0.0));
+      new Transform2d(new Translation2d(-0.1585, 0.0), new Rotation2d(0.0));
 }

From 10f0dac586a49bb1cbe50885a98d2a4e10821009 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sun, 12 Apr 2026 19:28:43 -0400
Subject: [PATCH 085/102] A-Cycle fixes

---
 .../deploy/pathplanner/paths/A-Cycle1.path    | 50 ++++++------
 .../deploy/pathplanner/paths/A-Cycle2.path    | 78 +++++++++----------
 .../deploy/pathplanner/paths/B-Cycle1.path    | 10 +--
 .../deploy/pathplanner/paths/B-Cycle2.path    | 14 ++--
 src/main/deploy/pathplanner/settings.json     | 14 ++--
 5 files changed, 83 insertions(+), 83 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index c105af3f0..11b272093 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 4.295,
-        "y": 7.531
+        "x": 4.327227191413238,
+        "y": 7.369320214669052
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.365636024664715,
-        "y": 7.580018615477625
+        "x": 5.340843841537115,
+        "y": 7.412915747707522
       },
       "isLocked": false,
       "linkedName": null
@@ -20,28 +20,28 @@
         "y": 6.832881932021468
       },
       "prevControl": {
-        "x": 7.352205583793333,
-        "y": 7.527391495153063
+        "x": 7.370803269541251,
+        "y": 7.412129900536413
       },
       "nextControl": {
-        "x": 7.96399098844635,
-        "y": 5.729501726483708
+        "x": 7.961914258614824,
+        "y": 5.839539470347579
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.896815742397138,
-        "y": 4.0593381037567084
+        "x": 7.783237924865832,
+        "y": 4.984186046511627
       },
       "prevControl": {
-        "x": 7.990620981847936,
-        "y": 4.332081885610835
+        "x": 7.869597464645584,
+        "y": 5.341020494676783
       },
       "nextControl": {
-        "x": 7.809668414958673,
-        "y": 3.8059525593755392
+        "x": 7.721527918569696,
+        "y": 4.729202565831794
       },
       "isLocked": false,
       "linkedName": null
@@ -52,12 +52,12 @@
         "y": 7.369320214669052
       },
       "prevControl": {
-        "x": 7.167489045136595,
-        "y": 6.930742523290983
+        "x": 7.21326055178334,
+        "y": 7.274241428662003
       },
       "nextControl": {
-        "x": 4.218132652871465,
-        "y": 7.689636218757288
+        "x": 4.101641571321129,
+        "y": 7.443273326241669
       },
       "isLocked": false,
       "linkedName": null
@@ -68,12 +68,12 @@
         "y": 7.369320214669052
       },
       "prevControl": {
-        "x": 3.755531829449267,
-        "y": 7.579788697952738
+        "x": 3.8027459218125412,
+        "y": 7.537383389137978
       },
       "nextControl": {
-        "x": 2.276559141756934,
-        "y": 6.698364760176492
+        "x": 2.207362527433732,
+        "y": 6.867684296448937
       },
       "isLocked": false,
       "linkedName": null
@@ -84,8 +84,8 @@
         "y": 4.553
       },
       "prevControl": {
-        "x": 2.412181093039758,
-        "y": 5.065191873649519
+        "x": 2.4792984729357204,
+        "y": 5.052513451565847
       },
       "nextControl": null,
       "isLocked": false,
@@ -120,7 +120,7 @@
       "minWaypointRelativePos": 1.0508474576271185,
       "maxWaypointRelativePos": 2.1694915254237293,
       "constraints": {
-        "maxVelocity": 1.2,
+        "maxVelocity": 1.7,
         "maxAcceleration": 6,
         "maxAngularVelocity": 540,
         "maxAngularAcceleration": 720,
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index 4532e53ee..76099a153 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.073045025903002,
-        "y": 5.010824871550651
+        "x": 2.9906316326262106,
+        "y": 5.102714219733314
       },
       "isLocked": false,
       "linkedName": "A-Cycle1-End"
@@ -20,76 +20,76 @@
         "y": 7.254742397137747
       },
       "prevControl": {
-        "x": 1.7727566660512677,
-        "y": 6.028530039315472
+        "x": 1.3305248035949697,
+        "y": 6.4381945652407
       },
       "nextControl": {
-        "x": 3.449050576638838,
-        "y": 7.414011757246001
+        "x": 3.5229482940373034,
+        "y": 7.367779487606841
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.4525328947368426,
-        "y": 7.254742397137747
+        "x": 6.6312343470483,
+        "y": 7.14216457960644
       },
       "prevControl": {
-        "x": 5.771047211855309,
-        "y": 7.83231450077766
+        "x": 5.0062651135040195,
+        "y": 7.6666279904228025
       },
       "nextControl": {
-        "x": 7.749507558733082,
-        "y": 6.1555316523359425
+        "x": 8.088080921525812,
+        "y": 6.671963231966102
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.327521929824561,
-        "y": 4.409032894736844
+        "x": 6.6312343470483,
+        "y": 4.091788908765653
       },
       "prevControl": {
-        "x": 6.559973370874717,
-        "y": 4.568129553133817
+        "x": 6.88316360390668,
+        "y": 4.291079662851518
       },
       "nextControl": {
-        "x": 5.871274488423377,
-        "y": 4.096763618189595
+        "x": 6.118695040000839,
+        "y": 3.686340386137116
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.779396929824561,
-        "y": 4.553276315789475
+        "x": 5.933542039355992,
+        "y": 4.28649373881932
       },
       "prevControl": {
-        "x": 5.751215757710489,
-        "y": 4.286409263322725
+        "x": 6.073073117998111,
+        "y": 4.07905440288477
       },
       "nextControl": {
-        "x": 5.805650962114751,
-        "y": 4.801893945471092
+        "x": 5.716749683104094,
+        "y": 4.608796575249948
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.779396929824561,
-        "y": 7.447760964912282
+        "x": 5.738837209302326,
+        "y": 7.254742397137747
       },
       "prevControl": {
-        "x": 6.769357739292765,
-        "y": 6.851722923583836
+        "x": 6.853018104342613,
+        "y": 6.708689690967459
       },
       "nextControl": {
-        "x": 5.565220720039658,
-        "y": 7.576712704575512
+        "x": 5.193957247675758,
+        "y": 7.521784471641628
       },
       "isLocked": false,
       "linkedName": null
@@ -100,24 +100,24 @@
         "y": 7.447760964912282
       },
       "prevControl": {
-        "x": 3.7599789090650377,
-        "y": 7.631925045537918
+        "x": 3.970192282405868,
+        "y": 7.43239065104425
       },
       "nextControl": {
-        "x": 2.9050441758844623,
-        "y": 7.3760898401707475
+        "x": 2.728196522051797,
+        "y": 7.455511892781865
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.548344298245614,
-        "y": 6.284197368421054
+        "x": 2.8831663685152056,
+        "y": 5.909033989266548
       },
       "prevControl": {
-        "x": 1.867329032576315,
-        "y": 6.919804106628433
+        "x": 1.8461433351002032,
+        "y": 6.781400700455524
       },
       "nextControl": null,
       "isLocked": false,
@@ -160,7 +160,7 @@
       "minWaypointRelativePos": 2.526032315978456,
       "maxWaypointRelativePos": 4.235188509874326,
       "constraints": {
-        "maxVelocity": 1.5,
+        "maxVelocity": 1.7,
         "maxAcceleration": 6,
         "maxAngularVelocity": 540,
         "maxAngularAcceleration": 720,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index 23d68c24c..74a18e9d7 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -132,11 +132,11 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4,
-    "maxAcceleration": 6,
-    "maxAngularVelocity": 540,
-    "maxAngularAcceleration": 720,
-    "nominalVoltage": 12,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 6.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path
index ee6cc4b5c..130a709c9 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path
@@ -37,7 +37,7 @@
       },
       "prevControl": {
         "x": 5.752347562091977,
-        "y": 0.24039328263318294
+        "y": 0.24039328263318283
       },
       "nextControl": {
         "x": 7.910712691624192,
@@ -89,7 +89,7 @@
       },
       "nextControl": {
         "x": 5.5662512875867165,
-        "y": 0.49059090002633776
+        "y": 0.49059090002633765
       },
       "isLocked": false,
       "linkedName": null
@@ -172,11 +172,11 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4,
-    "maxAcceleration": 6,
-    "maxAngularVelocity": 540,
-    "maxAngularAcceleration": 720,
-    "nominalVoltage": 12,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 6.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index d5a531bdd..65f9a1368 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -1,6 +1,6 @@
 {
-  "robotWidth": 0.702,
-  "robotLength": 0.702,
+  "robotWidth": 0.9017,
+  "robotLength": 0.8128,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
@@ -19,14 +19,14 @@
   "driveCurrentLimit": 46.0,
   "wheelCOF": 1.2,
   "flModuleX": 0.273,
-  "flModuleY": 0.244,
+  "flModuleY": 0.299,
   "frModuleX": 0.273,
-  "frModuleY": -0.244,
+  "frModuleY": -0.299,
   "blModuleX": -0.273,
-  "blModuleY": 0.244,
+  "blModuleY": 0.299,
   "brModuleX": -0.273,
-  "brModuleY": -0.244,
+  "brModuleY": -0.299,
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
-}
+}
\ No newline at end of file

From 858cfea13d43e74342481f15ef89a9d9a8eb718d Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Sun, 12 Apr 2026 18:29:05 -0500
Subject: [PATCH 086/102] rumble when intaking

---
 src/main/java/frc/robot/RobotContainer.java | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1bf04af26..c37b7dee3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -20,7 +20,6 @@
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -285,7 +284,11 @@ private void configureButtonBindings() {
             () -> -driverController.getRightX()));
 
     // Lock to 0 degrees when A button is held
-
+    Trigger intaking = new Trigger(() -> RobotState.getInstance().intaking);
+    intaking.onTrue(
+        Commands.runOnce(() -> driverController.setRumble(RumbleType.kBothRumble, 0.2)));
+    intaking.onFalse(
+        Commands.runOnce(() -> driverController.setRumble(RumbleType.kBothRumble, 0.0)));
     driverController
         .start()
         .onTrue(
@@ -319,11 +322,7 @@ private void configureButtonBindings() {
     // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
 
     driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp());
-    driverController
-        .leftTrigger(0.2)
-        .toggleOnTrue(
-            Commands.parallel(rumblePulse(0.5, 0.2), intake.runIntakeMotor())
-                .finallyDo(() -> CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15))));
+    driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor());
 
     driverController
         .a()

From 295ac022c0066cf16bbbed6359a2db031109b316 Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sun, 12 Apr 2026 19:30:30 -0400
Subject: [PATCH 087/102] Update settings.json

---
 src/main/deploy/pathplanner/settings.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 65f9a1368..7c4dbdbec 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -29,4 +29,4 @@
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
-}
\ No newline at end of file
+}

From 7abe7057ea396c74099e30b78533cb5c6554260d Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sun, 12 Apr 2026 19:45:02 -0400
Subject: [PATCH 088/102] A-Cycle fixes

---
 .../deploy/pathplanner/paths/A-Cycle1.path    | 48 ++++++------
 .../deploy/pathplanner/paths/A-Cycle2.path    | 74 +++++++++----------
 2 files changed, 61 insertions(+), 61 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index 11b272093..1c2c9ac6d 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -16,16 +16,16 @@
     },
     {
       "anchor": {
-        "x": 7.588533094812165,
-        "y": 6.832881932021468
+        "x": 7.783237924865832,
+        "y": 7.028586762075134
       },
       "prevControl": {
-        "x": 7.370803269541251,
-        "y": 7.412129900536413
+        "x": 7.767012522361359,
+        "y": 7.531574239713775
       },
       "nextControl": {
-        "x": 7.961914258614824,
-        "y": 5.839539470347579
+        "x": 7.817452345049281,
+        "y": 5.967939736388225
       },
       "isLocked": false,
       "linkedName": null
@@ -36,43 +36,43 @@
         "y": 4.984186046511627
       },
       "prevControl": {
-        "x": 7.869597464645584,
-        "y": 5.341020494676783
+        "x": 7.831914132379248,
+        "y": 5.324919499105545
       },
       "nextControl": {
-        "x": 7.721527918569696,
-        "y": 4.729202565831794
+        "x": 7.746136792727668,
+        "y": 4.724478121544485
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.463005366726297,
+        "x": 6.7285867620751345,
         "y": 7.369320214669052
       },
       "prevControl": {
-        "x": 7.21326055178334,
-        "y": 7.274241428662003
+        "x": 8.286225402504472,
+        "y": 7.35309481216458
       },
       "nextControl": {
-        "x": 4.101641571321129,
-        "y": 7.443273326241669
+        "x": 5.365289737415033,
+        "y": 7.383521225342595
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.402379248658318,
+        "x": 2.8669409660107332,
         "y": 7.369320214669052
       },
       "prevControl": {
-        "x": 3.8027459218125412,
+        "x": 3.2673076391649563,
         "y": 7.537383389137978
       },
       "nextControl": {
-        "x": 2.207362527433732,
+        "x": 1.671924244786147,
         "y": 6.867684296448937
       },
       "isLocked": false,
@@ -80,12 +80,12 @@
     },
     {
       "anchor": {
-        "x": 3.256,
-        "y": 4.553
+        "x": 2.753363148479427,
+        "y": 4.805706618962433
       },
       "prevControl": {
-        "x": 2.4792984729357204,
-        "y": 5.052513451565847
+        "x": 1.9766616214151478,
+        "y": 5.30522007052828
       },
       "nextControl": null,
       "isLocked": false,
@@ -99,11 +99,11 @@
     },
     {
       "waypointRelativePos": 1.171122994652407,
-      "rotationDegrees": -93.685
+      "rotationDegrees": -90.0
     },
     {
       "waypointRelativePos": 1.9411764705882377,
-      "rotationDegrees": -106.0
+      "rotationDegrees": -90.0
     },
     {
       "waypointRelativePos": 3.0160427807486627,
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index 76099a153..6a19f9a9b 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -3,108 +3,108 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.256350626118068,
-        "y": 4.553276315789475
+        "x": 2.753363148479427,
+        "y": 4.805706618962433
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.9906316326262106,
-        "y": 5.102714219733314
+        "x": 2.48764415498757,
+        "y": 5.355144522906272
       },
       "isLocked": false,
       "linkedName": "A-Cycle1-End"
     },
     {
       "anchor": {
-        "x": 3.256350626118068,
-        "y": 7.254742397137747
+        "x": 2.477531305903399,
+        "y": 7.447760964912282
       },
       "prevControl": {
-        "x": 1.3305248035949697,
-        "y": 6.4381945652407
+        "x": 2.039445438282648,
+        "y": 7.351408549885447
       },
       "nextControl": {
-        "x": 3.5229482940373034,
-        "y": 7.367779487606841
+        "x": 2.760343374071401,
+        "y": 7.509962528077773
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.6312343470483,
-        "y": 7.14216457960644
+        "x": 6.809713774597495,
+        "y": 7.447760964912282
       },
       "prevControl": {
-        "x": 5.0062651135040195,
-        "y": 7.6666279904228025
+        "x": 5.320357204173185,
+        "y": 7.530332248967907
       },
       "nextControl": {
-        "x": 8.088080921525812,
-        "y": 6.671963231966102
+        "x": 7.669660107334526,
+        "y": 7.400084757398864
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.6312343470483,
-        "y": 4.091788908765653
+        "x": 6.809713774597495,
+        "y": 3.9619856887298743
       },
       "prevControl": {
-        "x": 6.88316360390668,
-        "y": 4.291079662851518
+        "x": 7.0616430314558745,
+        "y": 4.16127644281574
       },
       "nextControl": {
-        "x": 6.118695040000839,
-        "y": 3.686340386137116
+        "x": 6.297174467550033,
+        "y": 3.556537166101337
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.933542039355992,
-        "y": 4.28649373881932
+        "x": 6.06334525939177,
+        "y": 4.513649373881932
       },
       "prevControl": {
-        "x": 6.073073117998111,
-        "y": 4.07905440288477
+        "x": 6.202876338033889,
+        "y": 4.306210037947382
       },
       "nextControl": {
-        "x": 5.716749683104094,
-        "y": 4.608796575249948
+        "x": 5.846552903139872,
+        "y": 4.83595221031256
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.738837209302326,
-        "y": 7.254742397137747
+        "x": 5.771288014311271,
+        "y": 7.447760964912282
       },
       "prevControl": {
-        "x": 6.853018104342613,
-        "y": 6.708689690967459
+        "x": 6.858389982110913,
+        "y": 7.448760964912282
       },
       "nextControl": {
-        "x": 5.193957247675758,
-        "y": 7.521784471641628
+        "x": 5.164488763675691,
+        "y": 7.447202784292056
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.1445504385964913,
+        "x": 3.142772808586762,
         "y": 7.447760964912282
       },
       "prevControl": {
-        "x": 3.970192282405868,
+        "x": 3.9684146523961386,
         "y": 7.43239065104425
       },
       "nextControl": {
-        "x": 2.728196522051797,
+        "x": 2.7264188920420676,
         "y": 7.455511892781865
       },
       "isLocked": false,

From 15e272d1934d333c469d02fb91bfc7cd25fba04c Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Sun, 12 Apr 2026 20:06:04 -0400
Subject: [PATCH 089/102] A-Cycle fixes

---
 .../deploy/pathplanner/paths/A-Cycle1.path    | 22 +++++++++----------
 .../deploy/pathplanner/paths/A-Cycle2.path    |  6 ++---
 2 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index 1c2c9ac6d..3291c8ee5 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 4.327227191413238,
+        "x": 4.424579606440071,
         "y": 7.369320214669052
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.340843841537115,
+        "x": 5.438196256563948,
         "y": 7.412915747707522
       },
       "isLocked": false,
@@ -33,15 +33,15 @@
     {
       "anchor": {
         "x": 7.783237924865832,
-        "y": 4.984186046511627
+        "y": 4.254042933810375
       },
       "prevControl": {
         "x": 7.831914132379248,
-        "y": 5.324919499105545
+        "y": 4.594776386404293
       },
       "nextControl": {
         "x": 7.746136792727668,
-        "y": 4.724478121544485
+        "y": 3.9943350088432332
       },
       "isLocked": false,
       "linkedName": null
@@ -49,30 +49,30 @@
     {
       "anchor": {
         "x": 6.7285867620751345,
-        "y": 7.369320214669052
+        "y": 7.4991234347048294
       },
       "prevControl": {
         "x": 8.286225402504472,
-        "y": 7.35309481216458
+        "y": 7.482898032200357
       },
       "nextControl": {
         "x": 5.365289737415033,
-        "y": 7.383521225342595
+        "y": 7.513324445378372
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.8669409660107332,
+        "x": 2.753363148479427,
         "y": 7.369320214669052
       },
       "prevControl": {
-        "x": 3.2673076391649563,
+        "x": 3.1537298216336502,
         "y": 7.537383389137978
       },
       "nextControl": {
-        "x": 1.671924244786147,
+        "x": 1.558346427254841,
         "y": 6.867684296448937
       },
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index 6a19f9a9b..337e5cb27 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -157,10 +157,10 @@
   "constraintZones": [
     {
       "name": "Constraints Zone",
-      "minWaypointRelativePos": 2.526032315978456,
-      "maxWaypointRelativePos": 4.235188509874326,
+      "minWaypointRelativePos": 2.0644067796610166,
+      "maxWaypointRelativePos": 3.22711864406781,
       "constraints": {
-        "maxVelocity": 1.7,
+        "maxVelocity": 1.5,
         "maxAcceleration": 6,
         "maxAngularVelocity": 540,
         "maxAngularAcceleration": 720,

From 98d5085debe03f9f40f76ba91973e7abfa924884 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Mon, 13 Apr 2026 13:22:21 +0000
Subject: [PATCH 090/102] retract while shooting

---
 src/main/java/frc/robot/Orchestrator.java | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 93d7bb2c2..9a5773b48 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -27,6 +27,7 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeConstants;
 import frc.robot.subsystems.intake.rollers.IntakeRollers;
 import frc.robot.subsystems.magicCarpet.MagicCarpet;
 import frc.robot.subsystems.shooter.Shooter;
@@ -267,6 +268,22 @@ public Command spinUpShooter(double velocityRPM) {
     return shooter.setTargetVelocity(Rotations.per(Minute).of(velocityRPM));
   }
 
+  public Command shootWhileRetractingIntake(Command shooterCommand) {
+    return Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            shooterCommand,
+            feedUp())
+        .withName("shootWhileRetractingIntake");
+  }
+
+  public Command shootWhileRetractingIntakeHub() {
+    return shootWhileRetractingIntake(spinUpShooterHub());
+  }
+
+  public Command shootWhileRetractingIntakeDistance(Supplier<Distance> targetDistance) {
+    return shootWhileRetractingIntake(spinUpShooterDistance(targetDistance));
+  }
+
   public Command driveShootAtAngle() {
     return Commands.parallel(
         Commands.run(

From ad17575d1619c22643e5b9d3d4a88b9f92987fc8 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Mon, 13 Apr 2026 16:24:49 -0500
Subject: [PATCH 091/102] new leds and autos

---
 src/main/deploy/pathplanner/paths/A-Cycle1.path   | 15 ++++++++++++++-
 src/main/deploy/pathplanner/paths/A-Cycle2.path   |  2 +-
 src/main/deploy/pathplanner/paths/B-Cycle1.path   |  2 +-
 src/main/deploy/pathplanner/paths/B-Cycle2.path   |  4 ++--
 src/main/deploy/pathplanner/settings.json         |  4 ++--
 src/main/java/frc/robot/Orchestrator.java         |  4 ++++
 src/main/java/frc/robot/RobotContainer.java       |  4 ++--
 src/main/java/frc/robot/commands/auto/Autos.java  |  3 ++-
 .../frc/robot/subsystems/leds/LedConstants.java   |  6 +++++-
 9 files changed, 33 insertions(+), 11 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index 3291c8ee5..bc79df9f0 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -127,12 +127,25 @@
         "nominalVoltage": 12,
         "unlimited": false
       }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 4.024307900067523,
+      "maxWaypointRelativePos": 4.956110735989194,
+      "constraints": {
+        "maxVelocity": 1.7,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
     }
   ],
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 2.5,
     "maxAcceleration": 6.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index 337e5cb27..75bf9b57c 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -172,7 +172,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 2.5,
     "maxAcceleration": 6.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index 74a18e9d7..db06b8000 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -132,7 +132,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 2.5,
     "maxAcceleration": 6.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path
index 130a709c9..c2d81162a 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path
@@ -89,7 +89,7 @@
       },
       "nextControl": {
         "x": 5.5662512875867165,
-        "y": 0.49059090002633765
+        "y": 0.4905909000263373
       },
       "isLocked": false,
       "linkedName": null
@@ -172,7 +172,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 2.5,
     "maxAcceleration": 6.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 7c4dbdbec..9f4a5d875 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -4,7 +4,7 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 4.0,
+  "defaultMaxVel": 2.5,
   "defaultMaxAccel": 6.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
@@ -29,4 +29,4 @@
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 93d7bb2c2..80da04768 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -305,4 +305,8 @@ public Command aimToHub() {
                             .getTranslation())
                     .getAngle()));
   }
+
+  public Command stopShootingAuto() {
+    return Commands.parallel(indexer.stop(), shooter.stop());
+  }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c37b7dee3..c37c543a6 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -119,7 +119,7 @@ public RobotContainer() {
                   new VisionIOPhotonVision(camera0Name, robotToCamera0),
                   new VisionIOPhotonVision(camera1Name, robotToCamera1),
                   new VisionIOPhotonVision(camera2Name, robotToCamera2));
-          //          leds = new Leds();
+          leds = new Leds();
           shooter = new Shooter(new ShooterIOSparkMax());
           magicCarpet = new MagicCarpet(new MagicCarpetSparkMax());
           indexer = new Indexer(new IndexerIOSparkMax());
@@ -186,7 +186,7 @@ public RobotContainer() {
         drive::setPose,
         drive::getChassisSpeeds,
         drive::runVelocity,
-        new PPHolonomicDriveController(new PIDConstants(12, 0, 0), new PIDConstants(9, 0, 0)),
+        new PPHolonomicDriveController(new PIDConstants(9.0, 0, 0), new PIDConstants(6, 0, 0)),
         ppConfig,
         () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
         drive);
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index b727ac44a..0ee125de8 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -195,7 +195,8 @@ private Command ppCycleRegressionConnect(String path) {
    */
   private Command pp2CycleRegression(String path1, String path2, Supplier<Pose2d> startPose) {
     return ppCycleRegression(path1, startPose)
-        .withTimeout(10.5)
+        .withTimeout(14.5)
+        .andThen(orchestrator.stopShootingAuto().withTimeout(0.1))
         .andThen(ppCycleRegressionConnect(path2));
   }
 
diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
index 66590c588..0f08a0e3e 100644
--- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java
+++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
@@ -7,7 +7,7 @@ public class LedConstants {
   public static final int LED_COUNT;
 
   /* num LEDs - change the first two numbers based on actual led strip */
-  public static final int FULL_LENGTH = 42; // 2026 robot LED count
+  public static final int FULL_LENGTH = 11; // 2026 robot LED count
   public static final int BAR_LENGTH = Math.min(10, FULL_LENGTH);
   public static final int BASE_LENGTH = (FULL_LENGTH - BAR_LENGTH) / 2;
 
@@ -46,6 +46,10 @@ public class LedConstants {
         LED_CHANNEL = 0;
         LED_COUNT = 14;
       }
+      case ROBOT_2026_COMP -> {
+        LED_CHANNEL = 0;
+        LED_COUNT = FULL_LENGTH;
+      }
       default -> {
         LED_CHANNEL = 0;
         LED_COUNT = 0;

From 58d32945dac4be37728204336c34cc2e450a087a Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Mon, 13 Apr 2026 19:07:28 -0400
Subject: [PATCH 092/102] A-Cycle fixes

---
 src/main/deploy/pathplanner/settings.json     | 12 ++--
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../java/frc/robot/commands/auto/Autos.java   | 72 ++++++++++++++++++-
 3 files changed, 76 insertions(+), 10 deletions(-)

diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 9f4a5d875..04d5ebb58 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -9,8 +9,8 @@
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 51.7095,
-  "robotMOI": 0.7,
+  "robotMass": 50.8,
+  "robotMOI": 0.1814,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.048,
   "driveGearing": 8.16,
@@ -19,13 +19,13 @@
   "driveCurrentLimit": 46.0,
   "wheelCOF": 1.2,
   "flModuleX": 0.273,
-  "flModuleY": 0.299,
+  "flModuleY": 0.244,
   "frModuleX": 0.273,
-  "frModuleY": -0.299,
+  "frModuleY": -0.244,
   "blModuleX": -0.273,
-  "blModuleY": 0.299,
+  "blModuleY": 0.244,
   "brModuleX": -0.273,
-  "brModuleY": -0.299,
+  "brModuleY": -0.244,
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c37c543a6..d3dc79dc5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -260,7 +260,7 @@ public RobotContainer() {
 
     // pp
 
-    autoChooser.addOption("A-side", autos.ppA2CycleRightRegression());
+    autoChooser.addOption("A-side", autos.ppACycleAlternate());
     autoChooser.addOption("B-side", autos.ppB2CycleRightRegression());
     // Configure the button bindings
     configureButtonBindings();
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 0ee125de8..b40133d2c 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -98,6 +98,45 @@ public Autos(
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000)));
   private static final Supplier<Pose2d> startBside =
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000)));
+  private static final Supplier<Pose2d> FrontBumpASide =
+      () ->
+          AllianceFlipUtil.apply(
+              new Pose2d(5.775780200958252, 5.496349811553955, new Rotation2d()));
+  private static final Supplier<Pose2d> BackBumpASide =
+      () -> AllianceFlipUtil.apply(new Pose2d(3.3, 5.496349811553955, new Rotation2d()));
+  private static final Supplier<Pose2d> BackTrenchASide =
+      () ->
+          AllianceFlipUtil.apply(
+              new Pose2d(
+                  3.3395299911499023,
+                  7.367389678955078,
+                  new Rotation2d(Units.degreesToRadians(-90))));
+  private static final Supplier<Pose2d> FrontTrenchASide =
+      () ->
+          AllianceFlipUtil.apply(
+              new Pose2d(5.9, 7.367389678955078, new Rotation2d(Units.degreesToRadians(-90))));
+  private static final Supplier<Pose2d> BackHubASide =
+      () ->
+          AllianceFlipUtil.apply(
+              new Pose2d(5.9, 4.367389678955078, new Rotation2d(Units.degreesToRadians(-90))));
+
+  private Command ppCycleAlternate(String path, Supplier<Pose2d> startPose) {
+    return Commands.sequence(
+        Commands.runOnce(() -> drive.setPose(startAside.get())),
+        Commands.deadline(
+                drive.getAutonomousCommand(path),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
+                orchestrator.spinUpShooterHub())
+            .withTimeout(14.5),
+        new StraightDriveToPose(drive, FrontBumpASide).withTimeout(2.0),
+        new StraightDriveToPose(drive, BackBumpASide).withTimeout(2.1),
+        orchestrator.aimToHub().withTimeout(2.5),
+        Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5),
+        Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooter(1214),
+            orchestrator.feedUp()));
+  }
 
   /**
    * Helper function for a single cycle auto with a constant shooting speed
@@ -132,9 +171,10 @@ private Command ppCycleConnect(String path) {
         orchestrator.aimToHub().withTimeout(2.5),
         Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5),
         Commands.parallel(
-            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-            orchestrator.spinUpShooter(1214),
-            orchestrator.feedUp()));
+                intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+                orchestrator.spinUpShooter(1214),
+                orchestrator.feedUp())
+            .withTimeout(2.5));
   }
 
   /**
@@ -200,6 +240,32 @@ private Command pp2CycleRegression(String path1, String path2, Supplier<Pose2d>
         .andThen(ppCycleRegressionConnect(path2));
   }
 
+  public Command ppACycleAlternate() {
+    return Commands.sequence(
+        ppCycleAlternate("A-Cycle1", startAside).withTimeout(10.5),
+        shooter.stop(),
+        new StraightDriveToPose(drive, BackTrenchASide).withTimeout(1.8),
+        new StraightDriveToPose(drive, FrontTrenchASide).withTimeout(1.8),
+        Commands.parallel(
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS),
+                new StraightDriveToPose(drive, BackHubASide))
+            .withTimeout(2.0),
+        new StraightDriveToPose(drive, FrontBumpASide).withTimeout(1.5),
+        Commands.parallel(
+                new StraightDriveToPose(drive, BackBumpASide),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()))
+            .withTimeout(3.0),
+        orchestrator.aimToHub().withTimeout(3),
+        Commands.parallel(
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                orchestrator.feedUp())
+            .withTimeout(2.0),
+        Commands.parallel(
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+            orchestrator.feedUp()));
+  }
+
   /**
    * Left side pathplanner auto for a single cycle with fixed shooting distance
    *

From ae2951040ad2e7deff599c45b53d3c312b7b37d2 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Mon, 13 Apr 2026 18:56:48 -0500
Subject: [PATCH 093/102] new autos

---
 .../deploy/pathplanner/paths/A-Cycle1.path    | 26 ++++----
 .../deploy/pathplanner/paths/A-Cycle2.path    | 66 +++++++++----------
 .../deploy/pathplanner/paths/B-Cycle1.path    |  4 +-
 .../deploy/pathplanner/paths/B-Cycle2.path    |  4 +-
 src/main/deploy/pathplanner/settings.json     |  6 +-
 src/main/java/frc/robot/Orchestrator.java     |  6 +-
 .../subsystems/drive/DriveConstants.java      |  2 +-
 .../subsystems/intake/IntakeConstants.java    |  2 +-
 .../robot/subsystems/leds/LedConstants.java   |  2 +-
 9 files changed, 58 insertions(+), 60 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index bc79df9f0..87ba06731 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -48,16 +48,16 @@
     },
     {
       "anchor": {
-        "x": 6.7285867620751345,
-        "y": 7.4991234347048294
+        "x": 6.797094017094016,
+        "y": 5.611267806267806
       },
       "prevControl": {
-        "x": 8.286225402504472,
-        "y": 7.482898032200357
+        "x": 8.354732657523353,
+        "y": 5.595042403763333
       },
       "nextControl": {
-        "x": 5.365289737415033,
-        "y": 7.513324445378372
+        "x": 5.433796992433915,
+        "y": 5.625468816941349
       },
       "isLocked": false,
       "linkedName": null
@@ -65,15 +65,15 @@
     {
       "anchor": {
         "x": 2.753363148479427,
-        "y": 7.369320214669052
+        "y": 5.611267806267806
       },
       "prevControl": {
-        "x": 3.1537298216336502,
-        "y": 7.537383389137978
+        "x": 3.1304660226829424,
+        "y": 5.826516791985741
       },
       "nextControl": {
-        "x": 1.558346427254841,
-        "y": 6.867684296448937
+        "x": 2.4817378917378914,
+        "y": 5.456225071225072
       },
       "isLocked": false,
       "linkedName": null
@@ -145,8 +145,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 6.0,
+    "maxVelocity": 1.8,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index 75bf9b57c..b7fcf9e69 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -48,76 +48,76 @@
     },
     {
       "anchor": {
-        "x": 6.809713774597495,
-        "y": 3.9619856887298743
+        "x": 7.171780626780627,
+        "y": 3.9962393162393166
       },
       "prevControl": {
-        "x": 7.0616430314558745,
-        "y": 4.16127644281574
+        "x": 7.270845035097081,
+        "y": 3.554259648365908
       },
       "nextControl": {
-        "x": 6.297174467550033,
-        "y": 3.556537166101337
+        "x": 7.003817663817664,
+        "y": 4.745612535612536
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.06334525939177,
-        "y": 4.513649373881932
+        "x": 6.616210826210827,
+        "y": 5.120299145299145
       },
       "prevControl": {
-        "x": 6.202876338033889,
-        "y": 4.306210037947382
+        "x": 7.094259259259258,
+        "y": 4.810213675213675
       },
       "nextControl": {
-        "x": 5.846552903139872,
-        "y": 4.83595221031256
+        "x": 6.183034659057372,
+        "y": 5.401278280750036
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.771288014311271,
-        "y": 7.447760964912282
+        "x": 5.28542735042735,
+        "y": 5.288262108262108
       },
       "prevControl": {
-        "x": 6.858389982110913,
-        "y": 7.448760964912282
+        "x": 6.435327635327635,
+        "y": 5.27534188034188
       },
       "nextControl": {
-        "x": 5.164488763675691,
-        "y": 7.447202784292056
+        "x": 4.6786661426501785,
+        "y": 5.295079649922526
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.142772808586762,
-        "y": 7.447760964912282
+        "x": 3.928803418803419,
+        "y": 5.288262108262108
       },
       "prevControl": {
-        "x": 3.9684146523961386,
-        "y": 7.43239065104425
+        "x": 4.754445262612792,
+        "y": 5.272891794394076
       },
       "nextControl": {
-        "x": 2.7264188920420676,
-        "y": 7.455511892781865
+        "x": 3.5124495022587245,
+        "y": 5.296013036131691
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.8831663685152056,
-        "y": 5.909033989266548
+        "x": 2.753363148479427,
+        "y": 4.805706618962433
       },
       "prevControl": {
-        "x": 1.8461433351002032,
-        "y": 6.781400700455524
+        "x": 3.42491452991453,
+        "y": 5.301182336182337
       },
       "nextControl": null,
       "isLocked": false,
@@ -139,19 +139,19 @@
     },
     {
       "waypointRelativePos": 2.9433962264150932,
-      "rotationDegrees": -150.0
+      "rotationDegrees": -89.41916770857625
     },
     {
       "waypointRelativePos": 3.9622641509434033,
-      "rotationDegrees": 80.0
+      "rotationDegrees": -1.3430615380779554
     },
     {
       "waypointRelativePos": 4.688679245283029,
-      "rotationDegrees": 90.0
+      "rotationDegrees": -1.3730703273820928
     },
     {
       "waypointRelativePos": 5.734276729559759,
-      "rotationDegrees": 90.0
+      "rotationDegrees": -0.749725876000854
     }
   ],
   "constraintZones": [
@@ -181,7 +181,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -51.09865544424842
+    "rotation": -20.612591739110343
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index db06b8000..dc478d301 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -132,8 +132,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 6.0,
+    "maxVelocity": 1.8,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path
index c2d81162a..3a3373947 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path
@@ -172,8 +172,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 6.0,
+    "maxVelocity": 1.8,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 9f4a5d875..8f4804d02 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -4,8 +4,8 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 2.5,
-  "defaultMaxAccel": 6.0,
+  "defaultMaxVel": 1.8,
+  "defaultMaxAccel": 3.5,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
@@ -29,4 +29,4 @@
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
-}
\ No newline at end of file
+}
diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 9e7e8f96b..1e38e5d1b 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -270,9 +270,7 @@ public Command spinUpShooter(double velocityRPM) {
 
   public Command shootWhileRetractingIntake(Command shooterCommand) {
     return Commands.parallel(
-            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-            shooterCommand,
-            feedUp())
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), shooterCommand, feedUp())
         .withName("shootWhileRetractingIntake");
   }
 
@@ -314,7 +312,7 @@ public Command aimToHub() {
                 drive.getPose().getX(),
                 drive.getPose().getY(),
                 AllianceFlipUtil.apply(Hub.blueCenter)
-                    .plus(new Translation2d(-0.6, 0))
+                    .plus(new Translation2d(-0.55, 0))
                     .minus(
                         drive
                             .getPose()
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index ce75e28ba..993178924 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -96,7 +96,7 @@ public class DriveConstants {
       (2 * PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec
 
   // Turn PID configuration
-  public static final double turnKp = 3.5;
+  public static final double turnKp = 12.0;
   public static final double turnKd = 0.0;
   public static final double turnSimP = 9.0;
   public static final double turnSimD = 0.0;
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index fd09d8c8c..6991842e1 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -33,6 +33,6 @@ public class IntakeConstants {
   public static final double SAFETY_TOLERANCE = 2;
   public static final double POSITION_TOLERANCE = 7;
 
-  public static final double INTAKE_EXTEND_MOTOR_CURRENT_LIMIT = 20;
+  public static final double INTAKE_EXTEND_MOTOR_CURRENT_LIMIT = 30;
   public static final double INTAKE_ROLLERS_MOTOR_CURRENT_LIMIT = 45;
 }
diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
index 0f08a0e3e..b37a506a3 100644
--- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java
+++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
@@ -7,7 +7,7 @@ public class LedConstants {
   public static final int LED_COUNT;
 
   /* num LEDs - change the first two numbers based on actual led strip */
-  public static final int FULL_LENGTH = 11; // 2026 robot LED count
+  public static final int FULL_LENGTH = 43; // 2026 robot LED count
   public static final int BAR_LENGTH = Math.min(10, FULL_LENGTH);
   public static final int BASE_LENGTH = (FULL_LENGTH - BAR_LENGTH) / 2;
 

From ff7b3eb4b1904e9687bdc855f9556bd824e7cd7e Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Mon, 13 Apr 2026 20:32:24 -0400
Subject: [PATCH 094/102] Revert "A-Cycle fixes"

This reverts commit 58d32945dac4be37728204336c34cc2e450a087a.
---
 src/main/deploy/pathplanner/settings.json     | 12 ++--
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../java/frc/robot/commands/auto/Autos.java   | 72 +------------------
 3 files changed, 10 insertions(+), 76 deletions(-)

diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 04d5ebb58..9f4a5d875 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -9,8 +9,8 @@
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 50.8,
-  "robotMOI": 0.1814,
+  "robotMass": 51.7095,
+  "robotMOI": 0.7,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.048,
   "driveGearing": 8.16,
@@ -19,13 +19,13 @@
   "driveCurrentLimit": 46.0,
   "wheelCOF": 1.2,
   "flModuleX": 0.273,
-  "flModuleY": 0.244,
+  "flModuleY": 0.299,
   "frModuleX": 0.273,
-  "frModuleY": -0.244,
+  "frModuleY": -0.299,
   "blModuleX": -0.273,
-  "blModuleY": 0.244,
+  "blModuleY": 0.299,
   "brModuleX": -0.273,
-  "brModuleY": -0.244,
+  "brModuleY": -0.299,
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d3dc79dc5..c37c543a6 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -260,7 +260,7 @@ public RobotContainer() {
 
     // pp
 
-    autoChooser.addOption("A-side", autos.ppACycleAlternate());
+    autoChooser.addOption("A-side", autos.ppA2CycleRightRegression());
     autoChooser.addOption("B-side", autos.ppB2CycleRightRegression());
     // Configure the button bindings
     configureButtonBindings();
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index b40133d2c..0ee125de8 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -98,45 +98,6 @@ public Autos(
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000)));
   private static final Supplier<Pose2d> startBside =
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000)));
-  private static final Supplier<Pose2d> FrontBumpASide =
-      () ->
-          AllianceFlipUtil.apply(
-              new Pose2d(5.775780200958252, 5.496349811553955, new Rotation2d()));
-  private static final Supplier<Pose2d> BackBumpASide =
-      () -> AllianceFlipUtil.apply(new Pose2d(3.3, 5.496349811553955, new Rotation2d()));
-  private static final Supplier<Pose2d> BackTrenchASide =
-      () ->
-          AllianceFlipUtil.apply(
-              new Pose2d(
-                  3.3395299911499023,
-                  7.367389678955078,
-                  new Rotation2d(Units.degreesToRadians(-90))));
-  private static final Supplier<Pose2d> FrontTrenchASide =
-      () ->
-          AllianceFlipUtil.apply(
-              new Pose2d(5.9, 7.367389678955078, new Rotation2d(Units.degreesToRadians(-90))));
-  private static final Supplier<Pose2d> BackHubASide =
-      () ->
-          AllianceFlipUtil.apply(
-              new Pose2d(5.9, 4.367389678955078, new Rotation2d(Units.degreesToRadians(-90))));
-
-  private Command ppCycleAlternate(String path, Supplier<Pose2d> startPose) {
-    return Commands.sequence(
-        Commands.runOnce(() -> drive.setPose(startAside.get())),
-        Commands.deadline(
-                drive.getAutonomousCommand(path),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
-                orchestrator.spinUpShooterHub())
-            .withTimeout(14.5),
-        new StraightDriveToPose(drive, FrontBumpASide).withTimeout(2.0),
-        new StraightDriveToPose(drive, BackBumpASide).withTimeout(2.1),
-        orchestrator.aimToHub().withTimeout(2.5),
-        Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5),
-        Commands.parallel(
-            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-            orchestrator.spinUpShooter(1214),
-            orchestrator.feedUp()));
-  }
 
   /**
    * Helper function for a single cycle auto with a constant shooting speed
@@ -171,10 +132,9 @@ private Command ppCycleConnect(String path) {
         orchestrator.aimToHub().withTimeout(2.5),
         Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5),
         Commands.parallel(
-                intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-                orchestrator.spinUpShooter(1214),
-                orchestrator.feedUp())
-            .withTimeout(2.5));
+            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
+            orchestrator.spinUpShooter(1214),
+            orchestrator.feedUp()));
   }
 
   /**
@@ -240,32 +200,6 @@ private Command pp2CycleRegression(String path1, String path2, Supplier<Pose2d>
         .andThen(ppCycleRegressionConnect(path2));
   }
 
-  public Command ppACycleAlternate() {
-    return Commands.sequence(
-        ppCycleAlternate("A-Cycle1", startAside).withTimeout(10.5),
-        shooter.stop(),
-        new StraightDriveToPose(drive, BackTrenchASide).withTimeout(1.8),
-        new StraightDriveToPose(drive, FrontTrenchASide).withTimeout(1.8),
-        Commands.parallel(
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS),
-                new StraightDriveToPose(drive, BackHubASide))
-            .withTimeout(2.0),
-        new StraightDriveToPose(drive, FrontBumpASide).withTimeout(1.5),
-        Commands.parallel(
-                new StraightDriveToPose(drive, BackBumpASide),
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()))
-            .withTimeout(3.0),
-        orchestrator.aimToHub().withTimeout(3),
-        Commands.parallel(
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                orchestrator.feedUp())
-            .withTimeout(2.0),
-        Commands.parallel(
-            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-            orchestrator.feedUp()));
-  }
-
   /**
    * Left side pathplanner auto for a single cycle with fixed shooting distance
    *

From a0e7e17792f52eb4d798f057520e7a117172efba Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Tue, 14 Apr 2026 17:09:27 -0400
Subject: [PATCH 095/102] spin up shooter on the way back

---
 src/main/java/frc/robot/Orchestrator.java        | 1 +
 src/main/java/frc/robot/commands/auto/Autos.java | 8 ++++++--
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 9e7e8f96b..20b4f9da8 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -260,6 +260,7 @@ public Command spinUpShooterDistance(Supplier<Distance> targetDistance) {
     return shooter.setTargetVelocity(shooter.calculateSetpoint(targetDistance));
   }
 
+
   public Command spinUpShooterHub() {
     return shooter.setTargetVelocity(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM));
   }
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 0ee125de8..5b9e7d56d 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -149,7 +149,9 @@ private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
         Commands.runOnce(() -> drive.setPose(startPose.get())),
         Commands.deadline(
                 drive.getAutonomousCommand(path),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
+                Commands.waitSeconds(12.5)
+                    .andThen(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())))
             .withTimeout(14.5),
         Commands.deadline(
             orchestrator.aimToHub().withTimeout(2.5),
@@ -168,7 +170,9 @@ private Command ppCycleRegressionConnect(String path) {
     return Commands.sequence(
         Commands.deadline(
                 drive.getAutonomousCommand(path),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
+                Commands.waitSeconds(12.5)
+                    .andThen(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())))
             .withTimeout(14.5),
         Commands.deadline(
             orchestrator.aimToHub().withTimeout(2.5),

From 85eeeb7c99641004e7f7490d94c82a5a75f52fde Mon Sep 17 00:00:00 2001
From: EJainDev <ekansh.jain2011@gmail.com>
Date: Tue, 14 Apr 2026 17:19:20 -0400
Subject: [PATCH 096/102] Revert "spin up shooter on the way back"

This reverts commit a0e7e17792f52eb4d798f057520e7a117172efba.
---
 src/main/java/frc/robot/Orchestrator.java        | 1 -
 src/main/java/frc/robot/commands/auto/Autos.java | 8 ++------
 2 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 20b4f9da8..9e7e8f96b 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -260,7 +260,6 @@ public Command spinUpShooterDistance(Supplier<Distance> targetDistance) {
     return shooter.setTargetVelocity(shooter.calculateSetpoint(targetDistance));
   }
 
-
   public Command spinUpShooterHub() {
     return shooter.setTargetVelocity(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM));
   }
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 5b9e7d56d..0ee125de8 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -149,9 +149,7 @@ private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
         Commands.runOnce(() -> drive.setPose(startPose.get())),
         Commands.deadline(
                 drive.getAutonomousCommand(path),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
-                Commands.waitSeconds(12.5)
-                    .andThen(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())))
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
             .withTimeout(14.5),
         Commands.deadline(
             orchestrator.aimToHub().withTimeout(2.5),
@@ -170,9 +168,7 @@ private Command ppCycleRegressionConnect(String path) {
     return Commands.sequence(
         Commands.deadline(
                 drive.getAutonomousCommand(path),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
-                Commands.waitSeconds(12.5)
-                    .andThen(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())))
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
             .withTimeout(14.5),
         Commands.deadline(
             orchestrator.aimToHub().withTimeout(2.5),

From 70e6f85ba16e26ff214027ef08049cba253b4800 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Tue, 14 Apr 2026 19:04:56 -0500
Subject: [PATCH 097/102] dance room regression and other cahnges

---
 .../deploy/pathplanner/paths/A-Cycle1.path    |  60 ++++------
 .../deploy/pathplanner/paths/A-Cycle2.path    | 110 +++++++++++-------
 .../deploy/pathplanner/paths/B-Cycle1.path    |   4 +-
 .../deploy/pathplanner/paths/B-Cycle2.path    |   4 +-
 src/main/deploy/pathplanner/settings.json     |   4 +-
 src/main/java/frc/robot/Orchestrator.java     |  43 ++++++-
 src/main/java/frc/robot/RobotContainer.java   |   7 +-
 .../java/frc/robot/commands/auto/Autos.java   |  48 ++++----
 .../frc/robot/subsystems/drive/Drive.java     |   2 +-
 .../subsystems/drive/DriveConstants.java      |   2 +-
 .../frc/robot/subsystems/intake/Intake.java   |   5 +
 .../subsystems/intake/IntakeConstants.java    |   3 +-
 .../frc/robot/subsystems/shooter/Shooter.java |   8 +-
 .../subsystems/shooter/ShooterConstants.java  |   4 +-
 .../subsystems/vision/VisionConstants.java    |   6 +-
 15 files changed, 188 insertions(+), 122 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path
index 87ba06731..6c49fd076 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path
@@ -33,15 +33,15 @@
     {
       "anchor": {
         "x": 7.783237924865832,
-        "y": 4.254042933810375
+        "y": 4.805706618962433
       },
       "prevControl": {
-        "x": 7.831914132379248,
-        "y": 4.594776386404293
+        "x": 7.77903133903134,
+        "y": 5.171980056980058
       },
       "nextControl": {
-        "x": 7.746136792727668,
-        "y": 3.9943350088432332
+        "x": 7.786250707881794,
+        "y": 4.543379297786823
       },
       "isLocked": false,
       "linkedName": null
@@ -52,40 +52,24 @@
         "y": 5.611267806267806
       },
       "prevControl": {
-        "x": 8.354732657523353,
-        "y": 5.595042403763333
+        "x": 7.802016105528991,
+        "y": 5.5605141654377555
       },
       "nextControl": {
-        "x": 5.433796992433915,
-        "y": 5.625468816941349
+        "x": 5.517991452991453,
+        "y": 5.675868945868947
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.753363148479427,
+        "x": 2.533418803418803,
         "y": 5.611267806267806
       },
       "prevControl": {
-        "x": 3.1304660226829424,
-        "y": 5.826516791985741
-      },
-      "nextControl": {
-        "x": 2.4817378917378914,
-        "y": 5.456225071225072
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.753363148479427,
-        "y": 4.805706618962433
-      },
-      "prevControl": {
-        "x": 1.9766616214151478,
-        "y": 5.30522007052828
+        "x": 3.334472934472934,
+        "y": 5.675868945868947
       },
       "nextControl": null,
       "isLocked": false,
@@ -106,11 +90,15 @@
       "rotationDegrees": -90.0
     },
     {
-      "waypointRelativePos": 3.0160427807486627,
+      "waypointRelativePos": 3.0,
       "rotationDegrees": 0.0
     },
     {
-      "waypointRelativePos": 3.8502673796791456,
+      "waypointRelativePos": 3.45,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 3.7526652452025653,
       "rotationDegrees": 0.0
     }
   ],
@@ -130,10 +118,10 @@
     },
     {
       "name": "Constraints Zone",
-      "minWaypointRelativePos": 4.024307900067523,
-      "maxWaypointRelativePos": 4.956110735989194,
+      "minWaypointRelativePos": 3.14382174206618,
+      "maxWaypointRelativePos": 3.792032410533432,
       "constraints": {
-        "maxVelocity": 1.7,
+        "maxVelocity": 2.3,
         "maxAcceleration": 6.0,
         "maxAngularVelocity": 540.0,
         "maxAngularAcceleration": 720.0,
@@ -145,8 +133,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.8,
-    "maxAcceleration": 3.5,
+    "maxVelocity": 8.0,
+    "maxAcceleration": 6.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -154,7 +142,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -23.436
+    "rotation": -37.18470645323313
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index b7fcf9e69..04a650ce2 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -3,28 +3,28 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.753363148479427,
-        "y": 4.805706618962433
+        "x": 2.533418803418803,
+        "y": 5.611267806267806
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.48764415498757,
-        "y": 5.355144522906272
+        "x": 2.267699809926946,
+        "y": 6.160705710211645
       },
       "isLocked": false,
       "linkedName": "A-Cycle1-End"
     },
     {
       "anchor": {
-        "x": 2.477531305903399,
+        "x": 2.5075783475783475,
         "y": 7.447760964912282
       },
       "prevControl": {
-        "x": 2.039445438282648,
+        "x": 2.0694924799575967,
         "y": 7.351408549885447
       },
       "nextControl": {
-        "x": 2.760343374071401,
+        "x": 2.7903904157463497,
         "y": 7.509962528077773
       },
       "isLocked": false,
@@ -32,15 +32,15 @@
     },
     {
       "anchor": {
-        "x": 6.809713774597495,
+        "x": 5.634273504273503,
         "y": 7.447760964912282
       },
       "prevControl": {
-        "x": 5.320357204173185,
+        "x": 4.144916933849193,
         "y": 7.530332248967907
       },
       "nextControl": {
-        "x": 7.669660107334526,
+        "x": 6.494219837010534,
         "y": 7.400084757398864
       },
       "isLocked": false,
@@ -48,76 +48,76 @@
     },
     {
       "anchor": {
-        "x": 7.171780626780627,
-        "y": 3.9962393162393166
+        "x": 6.202763532763532,
+        "y": 4.060840455840457
       },
       "prevControl": {
-        "x": 7.270845035097081,
-        "y": 3.554259648365908
+        "x": 6.215683760683759,
+        "y": 3.569871794871795
       },
       "nextControl": {
-        "x": 7.003817663817664,
-        "y": 4.745612535612536
+        "x": 6.1825608953181,
+        "y": 4.828540678766911
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.616210826210827,
-        "y": 5.120299145299145
+        "x": 5.866837606837606,
+        "y": 5.611
       },
       "prevControl": {
-        "x": 7.094259259259258,
-        "y": 4.810213675213675
+        "x": 6.590370370370371,
+        "y": 5.624188034188035
       },
       "nextControl": {
-        "x": 6.183034659057372,
-        "y": 5.401278280750036
+        "x": 5.351084085527113,
+        "y": 5.601599216214575
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.28542735042735,
-        "y": 5.288262108262108
+        "x": 4.755698005698005,
+        "y": 5.611
       },
       "prevControl": {
-        "x": 6.435327635327635,
-        "y": 5.27534188034188
+        "x": 5.905598290598291,
+        "y": 5.598079772079772
       },
       "nextControl": {
-        "x": 4.6786661426501785,
-        "y": 5.295079649922526
+        "x": 4.148936797920834,
+        "y": 5.6178175416604175
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.928803418803419,
-        "y": 5.288262108262108
+        "x": 3.760840455840455,
+        "y": 5.611
       },
       "prevControl": {
-        "x": 4.754445262612792,
-        "y": 5.272891794394076
+        "x": 4.6006552706552695,
+        "y": 5.598347578347578
       },
       "nextControl": {
-        "x": 3.5124495022587245,
-        "y": 5.296013036131691
+        "x": 3.34446165048371,
+        "y": 5.61727304986715
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.753363148479427,
-        "y": 4.805706618962433
+        "x": 2.533418803418803,
+        "y": 5.611267806267806
       },
       "prevControl": {
-        "x": 3.42491452991453,
-        "y": 5.301182336182337
+        "x": 3.476099715099715,
+        "y": 5.624455840455841
       },
       "nextControl": null,
       "isLocked": false,
@@ -151,7 +151,7 @@
     },
     {
       "waypointRelativePos": 5.734276729559759,
-      "rotationDegrees": -0.749725876000854
+      "rotationDegrees": -0.7497258760008542
     }
   ],
   "constraintZones": [
@@ -167,12 +167,38 @@
         "nominalVoltage": 12,
         "unlimited": false
       }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.529372045914936,
+      "maxWaypointRelativePos": 1.380148548278184,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 4.442943956785955,
+      "maxWaypointRelativePos": 6.4280891289669135,
+      "constraints": {
+        "maxVelocity": 2.3,
+        "maxAcceleration": 6.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
     }
   ],
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.5,
+    "maxVelocity": 8.0,
     "maxAcceleration": 6.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -181,13 +207,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -20.612591739110343
+    "rotation": -37.195226690262515
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -23.4357656437963
+    "rotation": -37.18470645323313
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index dc478d301..e8711eefc 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -132,8 +132,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.8,
-    "maxAcceleration": 3.5,
+    "maxVelocity": 8.0,
+    "maxAcceleration": 6.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path
index 3a3373947..dccdb4e65 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path
@@ -172,8 +172,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.8,
-    "maxAcceleration": 3.5,
+    "maxVelocity": 8.0,
+    "maxAcceleration": 6.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
index 8f4804d02..2e5cfd4e0 100644
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -4,8 +4,8 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 1.8,
-  "defaultMaxAccel": 3.5,
+  "defaultMaxVel": 8.0,
+  "defaultMaxAccel": 6.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index 1e38e5d1b..37135b86b 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -142,6 +142,47 @@ public ZoneId getCurrentZone() {
     return ZoneId.NONE;
   }
 
+  public Command zoneBasedShooter() {
+    DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY());
+
+    return new SelectCommand<>(
+        Map.ofEntries(
+            Map.entry(ZoneId.ZONE_1, spinUpShooterDistance(getHubDistance())),
+            Map.entry(
+                ZoneId.ZONE_2,
+                new ConditionalCommand(
+                    spinUpShooterDistance(
+                        () ->
+                            Meters.of(
+                                AllianceFlipUtil.apply(BBumpClosePose)
+                                    .getTranslation()
+                                    .getDistance((drive.getPose().getTranslation())))),
+                    spinUpShooterDistance(
+                        () ->
+                            Meters.of(
+                                AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpClosePose))
+                                    .getTranslation()
+                                    .getDistance(drive.getPose().getTranslation()))),
+                    () -> allianceY.getAsDouble() > FieldConstants.fieldWidth / 2)),
+            Map.entry(
+                ZoneId.ZONE_3,
+                spinUpShooterDistance(
+                    () ->
+                        Meters.of(
+                            AllianceFlipUtil.apply(BBumpFarPose)
+                                .getTranslation()
+                                .getDistance(drive.getPose().getTranslation())))),
+            Map.entry(
+                ZoneId.ZONE_4,
+                spinUpShooterDistance(
+                    () ->
+                        Meters.of(
+                            AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpFarPose))
+                                .getTranslation()
+                                .getDistance(drive.getPose().getTranslation()))))),
+        this::getCurrentZone);
+  }
+
   public Command zoneBasedAim() {
     DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY());
 
@@ -312,7 +353,7 @@ public Command aimToHub() {
                 drive.getPose().getX(),
                 drive.getPose().getY(),
                 AllianceFlipUtil.apply(Hub.blueCenter)
-                    .plus(new Translation2d(-0.55, 0))
+                    .plus(new Translation2d(-0.4, 0))
                     .minus(
                         drive
                             .getPose()
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c37c543a6..a205ec02e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -186,7 +186,7 @@ public RobotContainer() {
         drive::setPose,
         drive::getChassisSpeeds,
         drive::runVelocity,
-        new PPHolonomicDriveController(new PIDConstants(9.0, 0, 0), new PIDConstants(6, 0, 0)),
+        new PPHolonomicDriveController(new PIDConstants(14.0, 0, 0), new PIDConstants(7, 0, 0)),
         ppConfig,
         () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
         drive);
@@ -300,7 +300,7 @@ private void configureButtonBindings() {
                 .ignoringDisable(true));
     new Trigger(() -> driverController.getHID().getPOV() != -1)
         .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV()));
-    driverController.x().toggleOnTrue(orchestrator.aimToHub());
+    driverController.x().toggleOnTrue(orchestrator.zoneBasedAim());
     driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS));
     driverController
         .leftBumper()
@@ -341,8 +341,7 @@ private void configureButtonBindings() {
     operatorController
         .rightTrigger(0.1)
         .and(() -> !operatorController.pov(0).getAsBoolean())
-        .toggleOnTrue(
-            orchestrator.spinUpShooterDistance(orchestrator.getShootWhileDrivingResultDistance()));
+        .toggleOnTrue(orchestrator.zoneBasedShooter());
     operatorController
         .rightTrigger(0.1)
         .and(operatorController.pov(0))
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index 0ee125de8..a2cb262c9 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -12,6 +12,7 @@
 import frc.robot.subsystems.intake.IntakeConstants;
 import frc.robot.subsystems.intake.rollers.IntakeRollers;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
 import java.util.function.Supplier;
 
 /** Contains all autos */
@@ -149,38 +150,39 @@ private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
         Commands.runOnce(() -> drive.setPose(startPose.get())),
         Commands.deadline(
                 drive.getAutonomousCommand(path),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
-            .withTimeout(14.5),
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
+                Commands.waitSeconds(5)
+                    .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM)))
+            .withTimeout(15.5),
         Commands.deadline(
-            orchestrator.aimToHub().withTimeout(2.5),
-            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
-        Commands.parallel(
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                orchestrator.feedUp())
-            .withTimeout(2.5),
-        Commands.parallel(
-            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-            orchestrator.feedUp()));
+                orchestrator.aimToHub().withTimeout(1).withTimeout(2.5),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()))
+            .andThen(
+                Commands.parallel(
+                        Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()),
+                        orchestrator.aimToHub().withTimeout(1).repeatedly(),
+                        orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                        Commands.waitSeconds(0.4).andThen(orchestrator.feedUp()))
+                    .withTimeout(1.6)));
   }
 
   private Command ppCycleRegressionConnect(String path) {
     return Commands.sequence(
         Commands.deadline(
                 drive.getAutonomousCommand(path),
-                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5))
+                intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5),
+                Commands.waitSeconds(5)
+                    .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM)))
             .withTimeout(14.5),
         Commands.deadline(
-            orchestrator.aimToHub().withTimeout(2.5),
-            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())),
-        Commands.parallel(
-                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-                orchestrator.feedUp())
-            .withTimeout(2.5),
-        Commands.parallel(
-            intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS),
-            orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
-            orchestrator.feedUp()));
+                orchestrator.aimToHub().withTimeout(1).withTimeout(2.5),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()))
+            .andThen(
+                Commands.parallel(
+                    Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()),
+                    orchestrator.aimToHub().withTimeout(1).repeatedly(),
+                    orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                    Commands.waitSeconds(0.4).andThen(orchestrator.feedUp()))));
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 09d8613f1..4d405d38d 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -98,7 +98,7 @@ public Drive(
 
   @Override
   public void periodic() {
-    //    logCameraPositions(); // uncomment to show camera positions in advantage scope
+    logCameraPositions(); // uncomment to show camera positions in advantage scope
     odometryLock.lock(); // Prevents odometry updates while reading data
     gyroIO.updateInputs(gyroInputs);
     Logger.processInputs("Drive/Gyro", gyroInputs);
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index 993178924..a469e5c73 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -61,7 +61,7 @@ public class DriveConstants {
   public static final SwerveModuleConstants.ClosedLoopOutputType driveClosedLoopOutput =
       ClosedLoopOutputType.Voltage;
   public static final int driveMotorCurrentLimit = 50;
-  public static final double wheelRadiusMeters = Units.inchesToMeters(1.905);
+  public static final double wheelRadiusMeters = Units.inchesToMeters(2);
   public static final double driveMotorReduction = 8.16;
   public static final DCMotor driveGearbox = DCMotor.getKrakenX60Foc(1);
 
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index e1db70243..4d41fba74 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -29,6 +29,11 @@ public Command runIntakeMotor() {
     return rollers.intake();
   }
 
+  public Command slowlyBringInIntake() {
+    return Commands.parallel(rollers.intake(), extend.runIntakeExtendVolts(SLOW_VOLTS))
+        .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly());
+  }
+
   // Jack's Chugga Chugga mode
   public Command shakeAndIntake() {
     return Commands.repeatingSequence(
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 6991842e1..426486a16 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -18,7 +18,7 @@ public class IntakeConstants {
 
   public static final int EXTEND_LIMIT_ID = 0;
 
-  public static final double PID_P = 0.023; // arbritrary values
+  public static final double PID_P = 0.046; // arbritrary values
   public static final double PID_I = 0.000004;
   public static final double PID_D = 0.00001;
 
@@ -29,6 +29,7 @@ public class IntakeConstants {
   public static final double SHAKE_POS_OFFSET = 1;
   public static final double HOME_VOLTAGE = 3;
   public static final double COLLAPSE_POS = 4.0;
+  public static final double SLOW_VOLTS = 2;
   // How close we need to be to collapse position to stop the intake rollers
   public static final double SAFETY_TOLERANCE = 2;
   public static final double POSITION_TOLERANCE = 7;
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 569fd178f..80c857bb0 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -44,7 +44,7 @@ public class Shooter extends SubsystemBase {
 
   // Slew rate limiter: ramps the target velocity gradually (rad/s per second)
   // This prevents current spikes that cause oscillation with a 20A limit
-  private final SlewRateLimiter targetRamper = new SlewRateLimiter(1600);
+  private final SlewRateLimiter targetRamper = new SlewRateLimiter(800);
   private double feedForwardScalar;
 
   /**
@@ -263,7 +263,11 @@ public Time getAirTimeSeconds(Distance distance) {
   public Supplier<AngularVelocity> calculateSetpoint(Supplier<Distance> distance) {
     // calculate rpm depending on distance
     return () -> {
-      AngularVelocity velocity = RPM.of(102.4367 * distance.get().in(Meters) + 799.60799);
+      AngularVelocity velocity =
+          RPM.of(
+              25.30184 * Math.pow(distance.get().in(Meters), 2)
+                  - 47.12642 * distance.get().in(Meters)
+                  + 1001.70713);
       if (velocity.gt(RPM.of(1550))) {
         return RPM.of(1550);
       } else return velocity;
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index e2e0cd5f3..f784e5bea 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -11,7 +11,7 @@ public class ShooterConstants {
   public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60;
 
   public static final double VOLTAGE_COMPENSATION = 12.0;
-  public static final int CURRENT_LIMIT = 55;
+  public static final int CURRENT_LIMIT = 35;
   public static final IdleMode IDLE_MODE = IdleMode.kCoast;
 
   public static final double KV = 0.0662;
@@ -25,7 +25,7 @@ public class ShooterConstants {
   public static final double FF_SCALAR = 1;
 
   public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5;
-  public static final double CLOSE_HUB_SHOOTER_RPM = 850;
+  public static final double CLOSE_HUB_SHOOTER_RPM = 1100;
   public static final double MAX_VOLTAGE = 12.0;
 
   public static final double TOLERANCE =
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
index 109cc6887..463e6ce91 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -24,7 +24,7 @@ public class VisionConstants {
   private static Rotation3d RotationCorrection =
       new Rotation3d(0, 0, Math.PI / 2); // 90 degree roation around z-axis
   public static Transform3d robotToCamera0 =
-      new Transform3d( // front camera
+      new Transform3d(
           new Translation3d(
                   Units.inchesToMeters(-8.931),
                   Units.inchesToMeters(12.349),
@@ -35,7 +35,7 @@ public class VisionConstants {
               Units.degreesToRadians(0.0),
               Units.degreesToRadians(-158.771)));
   public static Transform3d robotToCamera1 =
-      new Transform3d( // rear left camera
+      new Transform3d(
           new Translation3d(
                   Units.inchesToMeters(8.931), // x
                   Units.inchesToMeters(12.349), // y
@@ -44,7 +44,7 @@ public class VisionConstants {
           new Rotation3d(
               Units.degreesToRadians(0.0),
               Units.degreesToRadians(0.0),
-              Units.degreesToRadians(-201.229)));
+              Units.degreesToRadians(158.771)));
 
   public static Transform3d robotToCamera2 =
       new Transform3d( // rear right camera

From b3767b5192dc790f3e1f81ac4d89a1bd7afa3455 Mon Sep 17 00:00:00 2001
From: Pranav Gundu <pranav@gundu.me>
Date: Tue, 14 Apr 2026 22:05:47 -0400
Subject: [PATCH 098/102] b side autos

---
 .../deploy/pathplanner/paths/A-Cycle2.path    |   8 +-
 .../deploy/pathplanner/paths/B-Cycle1.path    | 109 ++++++-------
 .../deploy/pathplanner/paths/B-Cycle2.path    | 144 +++++++++++-------
 3 files changed, 144 insertions(+), 117 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path
index 04a650ce2..811891d20 100644
--- a/src/main/deploy/pathplanner/paths/A-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path
@@ -139,19 +139,19 @@
     },
     {
       "waypointRelativePos": 2.9433962264150932,
-      "rotationDegrees": -89.41916770857625
+      "rotationDegrees": -90.0
     },
     {
       "waypointRelativePos": 3.9622641509434033,
-      "rotationDegrees": -1.3430615380779554
+      "rotationDegrees": 0.0
     },
     {
       "waypointRelativePos": 4.688679245283029,
-      "rotationDegrees": -1.3730703273820928
+      "rotationDegrees": 0.0
     },
     {
       "waypointRelativePos": 5.734276729559759,
-      "rotationDegrees": -0.7497258760008542
+      "rotationDegrees": 0.0
     }
   ],
   "constraintZones": [
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path
index e8711eefc..632dc384e 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle1.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path
@@ -3,93 +3,77 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 4.295,
-        "y": 0.5380000000000011
+        "x": 4.424579606440071,
+        "y": 0.6996797853309484
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.260984348859008,
-        "y": 0.548709162061195
+        "x": 5.438196256563948,
+        "y": 0.6560842522924792
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.588533094812165,
-        "y": 1.2361180679785324
+        "x": 7.783237924865832,
+        "y": 1.0404132379248665
       },
       "prevControl": {
-        "x": 7.333281138772382,
-        "y": 0.5372671705796467
+        "x": 7.767012522361359,
+        "y": 0.5374257602862258
       },
       "nextControl": {
-        "x": 8.01198853278679,
-        "y": 2.3954910162934935
+        "x": 7.817452345049281,
+        "y": 2.1010602636117754
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.896815742397138,
-        "y": 4.009661896243292
+        "x": 7.783237924865832,
+        "y": 3.2632933810375677
       },
       "prevControl": {
-        "x": 7.956228852277296,
-        "y": 3.7035208327957765
+        "x": 7.77903133903134,
+        "y": 2.8970199430199433
       },
       "nextControl": {
-        "x": 7.8491866370218535,
-        "y": 4.255082898443899
+        "x": 7.786250707881794,
+        "y": 3.5256207022131782
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.463005366726297,
-        "y": 0.6996797853309484
-      },
-      "prevControl": {
-        "x": 7.033869255673328,
-        "y": 1.1479768709205327
-      },
-      "nextControl": {
-        "x": 4.179018869195575,
-        "y": 0.33325249087882547
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.402379248658318,
-        "y": 0.6996797853309484
+        "x": 6.797094017094016,
+        "y": 2.4577321937321948
       },
       "prevControl": {
-        "x": 3.6989618406989995,
-        "y": 0.5442676850187853
+        "x": 7.802016105528991,
+        "y": 2.5084858345622454
       },
       "nextControl": {
-        "x": 2.3915233528638398,
-        "y": 1.2293778933601753
+        "x": 5.517991452991453,
+        "y": 2.3931310541310538
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.451,
-        "y": 3.264
+        "x": 2.533418803418803,
+        "y": 2.4577321937321948
       },
       "prevControl": {
-        "x": 2.5377853397080075,
-        "y": 2.6727592008119645
+        "x": 3.334472934472934,
+        "y": 2.3931310541310538
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "B-Cycle-End"
+      "linkedName": "A-Cycle1-End"
     }
   ],
   "rotationTargets": [
@@ -99,18 +83,22 @@
     },
     {
       "waypointRelativePos": 1.171122994652407,
-      "rotationDegrees": 86.315
+      "rotationDegrees": 90.0
     },
     {
       "waypointRelativePos": 1.9411764705882377,
-      "rotationDegrees": 94.0
+      "rotationDegrees": 90.0
     },
     {
-      "waypointRelativePos": 3.0160427807486627,
+      "waypointRelativePos": 3,
       "rotationDegrees": 0.0
     },
     {
-      "waypointRelativePos": 3.8502673796791456,
+      "waypointRelativePos": 3.45,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 3.7526652452025653,
       "rotationDegrees": 0.0
     }
   ],
@@ -120,7 +108,20 @@
       "minWaypointRelativePos": 1.0508474576271185,
       "maxWaypointRelativePos": 2.1694915254237293,
       "constraints": {
-        "maxVelocity": 1.2,
+        "maxVelocity": 1.7,
+        "maxAcceleration": 6,
+        "maxAngularVelocity": 540,
+        "maxAngularAcceleration": 720,
+        "nominalVoltage": 12,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 3.14382174206618,
+      "maxWaypointRelativePos": 3.792032410533432,
+      "constraints": {
+        "maxVelocity": 2.3,
         "maxAcceleration": 6,
         "maxAngularVelocity": 540,
         "maxAngularAcceleration": 720,
@@ -132,16 +133,16 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 8.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "maxVelocity": 8,
+    "maxAcceleration": 6,
+    "maxAngularVelocity": 540,
+    "maxAngularAcceleration": 720,
+    "nominalVoltage": 12,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 37.717000000000006
+    "rotation": -37.18470645323313
   },
   "reversed": false,
   "folder": null,
diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path
index dccdb4e65..a28ef683d 100644
--- a/src/main/deploy/pathplanner/paths/B-Cycle2.path
+++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path
@@ -3,121 +3,121 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.4510554561717353,
-        "y": 3.2642933810375667
+        "x": 2.533418803418803,
+        "y": 2.4577321937321948
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.2421167389384307,
-        "y": 2.967186063127149
+        "x": 2.267699809926946,
+        "y": 1.9082942897883557
       },
       "isLocked": false,
-      "linkedName": "B-Cycle-End"
+      "linkedName": "A-Cycle1-End"
     },
     {
       "anchor": {
-        "x": 3.256350626118068,
-        "y": 0.8142576028622539
+        "x": 2.5075783475783475,
+        "y": 0.6212390350877186
       },
       "prevControl": {
-        "x": 1.5889035900296817,
-        "y": 2.0483193166651414
+        "x": 2.0694924799575967,
+        "y": 0.717591450114554
       },
       "nextControl": {
-        "x": 3.457302404516393,
-        "y": 0.6655351003330456
+        "x": 2.7903904157463497,
+        "y": 0.5590374719222275
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.4525328947368426,
-        "y": 0.8142576028622539
+        "x": 5.634273504273503,
+        "y": 0.6212390350877186
       },
       "prevControl": {
-        "x": 5.752347562091977,
-        "y": 0.24039328263318283
+        "x": 4.144916933849193,
+        "y": 0.5386677510320939
       },
       "nextControl": {
-        "x": 7.910712691624192,
-        "y": 2.0093659818847356
+        "x": 6.494219837010534,
+        "y": 0.6689152426011367
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.327521929824561,
-        "y": 3.6599671052631573
+        "x": 6.202763532763532,
+        "y": 4.008159544159544
       },
       "prevControl": {
-        "x": 6.59100273960581,
-        "y": 3.4940005006714894
+        "x": 6.215683760683759,
+        "y": 4.499128205128206
       },
       "nextControl": {
-        "x": 5.920916329346852,
-        "y": 3.9160880386065315
+        "x": 6.1825608953181,
+        "y": 3.2404593212330894
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.779396929824561,
-        "y": 3.515723684210526
+        "x": 5.866837606837606,
+        "y": 2.458000000000001
       },
       "prevControl": {
-        "x": 5.837373947319441,
-        "y": 3.7589081110194775
+        "x": 6.590370370370371,
+        "y": 2.444811965811966
       },
       "nextControl": {
-        "x": 5.6945270228567875,
-        "y": 3.159737112788334
+        "x": 5.351084085527113,
+        "y": 2.4674007837854255
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.779396929824561,
-        "y": 0.6212390350877186
+        "x": 4.755698005698005,
+        "y": 2.458000000000001
       },
       "prevControl": {
-        "x": 6.736601761736383,
-        "y": 1.2079600435401154
+        "x": 5.905598290598291,
+        "y": 2.470920227920229
       },
       "nextControl": {
-        "x": 5.5662512875867165,
-        "y": 0.4905909000263373
+        "x": 4.148936797920834,
+        "y": 2.4511824583395834
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.1445504385964913,
-        "y": 0.6212390350877186
+        "x": 3.760840455840455,
+        "y": 2.458000000000001
       },
       "prevControl": {
-        "x": 3.5589239547141958,
-        "y": 0.44627660534504465
+        "x": 4.6006552706552695,
+        "y": 2.4706524216524226
       },
       "nextControl": {
-        "x": 2.8278583396709256,
-        "y": 0.7549570864425861
+        "x": 3.34446165048371,
+        "y": 2.4517269501328505
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.548344298245614,
-        "y": 1.7848026315789465
+        "x": 2.533418803418803,
+        "y": 2.4577321937321948
       },
       "prevControl": {
-        "x": 1.9675297251591926,
-        "y": 1.3628954494520977
+        "x": 3.476099715099715,
+        "y": 2.4445441595441597
       },
       "nextControl": null,
       "isLocked": false,
@@ -139,26 +139,26 @@
     },
     {
       "waypointRelativePos": 2.9433962264150932,
-      "rotationDegrees": 119.99999999999999
+      "rotationDegrees": 90.0
     },
     {
-      "waypointRelativePos": 3.962264150943403,
-      "rotationDegrees": -100.0
+      "waypointRelativePos": 3.9622641509434033,
+      "rotationDegrees": 0.0
     },
     {
       "waypointRelativePos": 4.688679245283029,
-      "rotationDegrees": -90.0
+      "rotationDegrees": 0.0
     },
     {
       "waypointRelativePos": 5.734276729559759,
-      "rotationDegrees": -90.0
+      "rotationDegrees": 0.0
     }
   ],
   "constraintZones": [
     {
       "name": "Constraints Zone",
-      "minWaypointRelativePos": 2.526032315978456,
-      "maxWaypointRelativePos": 4.235188509874326,
+      "minWaypointRelativePos": 2.0644067796610166,
+      "maxWaypointRelativePos": 3.22711864406781,
       "constraints": {
         "maxVelocity": 1.5,
         "maxAcceleration": 6,
@@ -167,27 +167,53 @@
         "nominalVoltage": 12,
         "unlimited": false
       }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.529372045914936,
+      "maxWaypointRelativePos": 1.380148548278184,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 6,
+        "maxAngularVelocity": 540,
+        "maxAngularAcceleration": 720,
+        "nominalVoltage": 12,
+        "unlimited": false
+      }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 4.442943956785955,
+      "maxWaypointRelativePos": 6.4280891289669135,
+      "constraints": {
+        "maxVelocity": 2.3,
+        "maxAcceleration": 6,
+        "maxAngularVelocity": 540,
+        "maxAngularAcceleration": 720,
+        "nominalVoltage": 12,
+        "unlimited": false
+      }
     }
   ],
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 8.0,
-    "maxAcceleration": 6.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
+    "maxVelocity": 8,
+    "maxAcceleration": 6,
+    "maxAngularVelocity": 540,
+    "maxAngularAcceleration": 720,
+    "nominalVoltage": 12,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 38.90134455575158
+    "rotation": 52.804773309737485
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 37.71655912222805
+    "rotation": -37.18470645323313
   },
   "useDefaultConstraints": true
 }
\ No newline at end of file

From ef99e0c6adab1a171211d0f92773e48893915d7c Mon Sep 17 00:00:00 2001
From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com>
Date: Wed, 15 Apr 2026 18:33:31 -0400
Subject: [PATCH 099/102] depot auto

---
 src/main/deploy/pathplanner/paths/Depot.path  | 109 ++++++++++++++++++
 .../java/frc/robot/commands/auto/Autos.java   |   7 ++
 2 files changed, 116 insertions(+)
 create mode 100644 src/main/deploy/pathplanner/paths/Depot.path

diff --git a/src/main/deploy/pathplanner/paths/Depot.path b/src/main/deploy/pathplanner/paths/Depot.path
new file mode 100644
index 000000000..68d658773
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Depot.path
@@ -0,0 +1,109 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.63,
+        "y": 5.844
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.9647584973166365,
+        "y": 5.90890161001789
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.282826475849731,
+        "y": 5.9577101967799635
+      },
+      "prevControl": {
+        "x": 3.2798809613513087,
+        "y": 5.881013697895228
+      },
+      "nextControl": {
+        "x": 1.8609660107334522,
+        "y": 5.990161001788907
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.5629338103756705,
+        "y": 5.9577101967799635
+      },
+      "prevControl": {
+        "x": 1.6650123660517429,
+        "y": 5.905230265557292
+      },
+      "nextControl": {
+        "x": 0.22220035778175273,
+        "y": 5.973935599284436
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.9480679785330945,
+        "y": 4.611001788908765
+      },
+      "prevControl": {
+        "x": 1.5202325581395346,
+        "y": 5.324919499105545
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0160427807486627,
+      "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 1.9358288770053491,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.799999999999997,
+      "maxWaypointRelativePos": 2.450847457627116,
+      "constraints": {
+        "maxVelocity": 3.58,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 8.0,
+    "maxAcceleration": 6.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -27.897271030947596
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index a2cb262c9..bffb1709e 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -99,6 +99,7 @@ public Autos(
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000)));
   private static final Supplier<Pose2d> startBside =
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000)));
+  private static final Supplier<Pose2d> startDepot = ()->AllianceFlipUtil.apply(new Pose2d(3.630,5.844,new Rotation2d(0.000)));
 
   /**
    * Helper function for a single cycle auto with a constant shooting speed
@@ -202,6 +203,12 @@ private Command pp2CycleRegression(String path1, String path2, Supplier<Pose2d>
         .andThen(ppCycleRegressionConnect(path2));
   }
 
+  /**
+   *
+   * Depot Auto
+   *
+   */
+  public Command ppDepot(){return ppCycleRegression("A-Cycle-LeftSweep",startDepot);};
   /**
    * Left side pathplanner auto for a single cycle with fixed shooting distance
    *

From 25f3db1f66f01d3c00b34f7a56ca209eb726baec Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Wed, 15 Apr 2026 17:34:45 -0500
Subject: [PATCH 100/102] Depo auto cycle command sequence

---
 .../java/frc/robot/commands/auto/Autos.java   | 21 +++++++++++++++++++
 .../subsystems/intake/IntakeConstants.java    |  2 +-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index a2cb262c9..75779d1d7 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -166,6 +166,27 @@ private Command ppCycleRegression(String path, Supplier<Pose2d> startPose) {
                     .withTimeout(1.6)));
   }
 
+  private Command ppCycleRegressionDepo(String path, Supplier<Pose2d> startPose) {
+    return Commands.sequence(
+        Commands.runOnce(() -> drive.setPose(startPose.get())),
+        Commands.deadline(
+                drive.getAutonomousCommand(path),
+                intake.extendToAngleAndIntake(IntakeConstants.FUNNEL_POS).withTimeout(5.5),
+                Commands.waitSeconds(5)
+                    .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM)))
+            .withTimeout(15.5),
+        Commands.deadline(
+                orchestrator.aimToHub().withTimeout(1).withTimeout(2.5),
+                orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()))
+            .andThen(
+                Commands.parallel(
+                        Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()),
+                        orchestrator.aimToHub().withTimeout(1).repeatedly(),
+                        orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()),
+                        Commands.waitSeconds(0.4).andThen(orchestrator.feedUp()))
+                    .withTimeout(1.6)));
+  }
+
   private Command ppCycleRegressionConnect(String path) {
     return Commands.sequence(
         Commands.deadline(
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 426486a16..263ce3b4d 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -23,7 +23,7 @@ public class IntakeConstants {
   public static final double PID_D = 0.00001;
 
   public static final double EXTEND_POS = -13; // TODO: change the actual values
-  public static final double FUNNEL_POS = -7;
+  public static final double FUNNEL_POS = -10;
 
   // shake around the funnel pos by this much
   public static final double SHAKE_POS_OFFSET = 1;

From a48e17cec21b770bc89f2dea3ea4e84accf1bf95 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Wed, 15 Apr 2026 17:43:12 -0500
Subject: [PATCH 101/102] better depo path

---
 src/main/deploy/pathplanner/paths/Depot.path | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/Depot.path b/src/main/deploy/pathplanner/paths/Depot.path
index 68d658773..90ed3dfca 100644
--- a/src/main/deploy/pathplanner/paths/Depot.path
+++ b/src/main/deploy/pathplanner/paths/Depot.path
@@ -32,16 +32,16 @@
     },
     {
       "anchor": {
-        "x": 0.5629338103756705,
+        "x": 0.685826210826211,
         "y": 5.9577101967799635
       },
       "prevControl": {
-        "x": 1.6650123660517429,
-        "y": 5.905230265557292
+        "x": 1.7065242165242167,
+        "y": 5.947193732193732
       },
       "nextControl": {
-        "x": 0.22220035778175273,
-        "y": 5.973935599284436
+        "x": 0.3447247623997972,
+        "y": 5.9612246361956025
       },
       "isLocked": false,
       "linkedName": null
@@ -52,8 +52,8 @@
         "y": 4.611001788908765
       },
       "prevControl": {
-        "x": 1.5202325581395346,
-        "y": 5.324919499105545
+        "x": 2.365455840455841,
+        "y": 6.218518518518518
       },
       "nextControl": null,
       "isLocked": false,
@@ -68,6 +68,10 @@
     {
       "waypointRelativePos": 1.9358288770053491,
       "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 2.660980810234532,
+      "rotationDegrees": 180.0
     }
   ],
   "constraintZones": [

From eba0a31609d793c83ec39b5156663c6342c7e479 Mon Sep 17 00:00:00 2001
From: Elliot Moore <eqm.emoore@hotmail.com>
Date: Wed, 15 Apr 2026 17:50:36 -0500
Subject: [PATCH 102/102] spotless apply

---
 src/main/java/frc/robot/commands/auto/Autos.java | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java
index beba3c9a5..06eb4f0a5 100644
--- a/src/main/java/frc/robot/commands/auto/Autos.java
+++ b/src/main/java/frc/robot/commands/auto/Autos.java
@@ -99,7 +99,8 @@ public Autos(
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000)));
   private static final Supplier<Pose2d> startBside =
       () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000)));
-  private static final Supplier<Pose2d> startDepot = ()->AllianceFlipUtil.apply(new Pose2d(3.630,5.844,new Rotation2d(0.000)));
+  private static final Supplier<Pose2d> startDepot =
+      () -> AllianceFlipUtil.apply(new Pose2d(3.630, 5.844, new Rotation2d(0.000)));
 
   /**
    * Helper function for a single cycle auto with a constant shooting speed
@@ -224,12 +225,12 @@ private Command pp2CycleRegression(String path1, String path2, Supplier<Pose2d>
         .andThen(ppCycleRegressionConnect(path2));
   }
 
-  /**
-   *
-   * Depot Auto
-   *
-   */
-  public Command ppDepot(){return ppCycleRegression("A-Cycle-LeftSweep",startDepot);};
+  /** Depot Auto */
+  public Command ppDepot() {
+    return ppCycleRegression("A-Cycle-LeftSweep", startDepot);
+  }
+  ;
+
   /**
    * Left side pathplanner auto for a single cycle with fixed shooting distance
    *