Merge pull request #41 from Team4388/Denver-comp

Just move stuff to master

src/main/deploy/pathplanner/autos/1 Coral Center Barge Left.auto

@@ -4,10 +4,16 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
         {
           "type": "path",
           "data": {
-            "pathName": "Top Blue Cage to Reef"
+            "pathName": "Center Barge to Reef 4 Left"
           }
         },
         {
@@ -16,34 +22,16 @@
             "name": "place-coral-left-l4"
           }
         },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Top Reef to Feed"
-          }
-        },
         {
           "type": "named",
           "data": {
-            "name": "grab-coral"
+            "name": "lower-algae-removal"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Top Feed to Reef"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral-left-l4"
           }
         }
       ]
     }
   },
   "resetOdom": true,
-  "folder": null,
+  "folder": "1 Coral",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/1 Coral Center Barge Right.auto

@@ -4,10 +4,16 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
         {
           "type": "path",
           "data": {
-            "pathName": "Bottom Red Cage to Reef"
+            "pathName": "Center Barge to Reef 4 Right"
           }
         },
         {
@@ -16,34 +22,16 @@
             "name": "place-coral-right-l4"
           }
         },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Bottom Reef to Feed"
-          }
-        },
         {
           "type": "named",
           "data": {
-            "name": "grab-coral"
+            "name": "lower-algae-removal"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Bottom Feed to Reef"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral-left-l4"
           }
         }
       ]
     }
   },
   "resetOdom": true,
-  "folder": null,
+  "folder": "1 Coral",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/2 Coral Cage 1.auto

@@ -4,28 +4,34 @@
     "type": "sequential",
     "data": {
       "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Center Red Cage to Reef"
-          }
-        },
         {
           "type": "named",
           "data": {
-            "name": "place-coral-right-l4"
+            "name": "prepare-l4"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "Bottom Reef to Feed"
+            "pathName": "Cage 1 to Reef"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "align-feed"
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 5 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
           }
         },
         {
@@ -37,19 +43,43 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Bottom Feed to Reef"
+            "pathName": "Top Feed to Reef 6"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "place-coral-left-l4"
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
           }
         }
       ]
     }
   },
   "resetOdom": true,
-  "folder": null,
+  "folder": "2 Coral",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/2 Coral Cage 2.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 2 to Reef 5"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 5 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Feed to Reef 6"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Cage 3.auto

@@ -4,28 +4,34 @@
     "type": "sequential",
     "data": {
       "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Center Barge to Reef"
-          }
-        },
         {
           "type": "named",
           "data": {
-            "name": "april-allign"
+            "name": "prepare-l4"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "Center Reef to Bottom Feed"
+            "pathName": "Cage 3 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 5 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
           }
         },
         {
@@ -37,25 +43,43 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Bottom Feed to Reef"
+            "pathName": "Top Feed to Reef 6"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "april-allign"
+            "name": "await-coral"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "place-coral"
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
           }
         }
       ]
     }
   },
   "resetOdom": true,
-  "folder": null,
+  "folder": "2 Coral",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/2 Coral Cage 4.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 4 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 3 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Cage 5.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 5 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 3 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Cage 6.auto

@@ -0,0 +1,85 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 6 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 3 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "2 Coral",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2 Coral Center Barge Left.auto

@@ -4,28 +4,34 @@
     "type": "sequential",
     "data": {
       "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Center Blue Cage to Reef"
-          }
-        },
         {
           "type": "named",
           "data": {
-            "name": "april-allign"
+            "name": "prepare-l4"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "Top Reef to Feed"
+            "pathName": "Center Barge to Reef 4 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 4 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
           }
         },
         {
@@ -37,25 +43,37 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Top Feed to Reef"
+            "pathName": "Top Feed to Reef 6"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "april-allign"
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "place-coral"
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
           }
         }
       ]
     }
   },
   "resetOdom": true,
-  "folder": null,
+  "folder": "2 Coral",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/2 Coral Center Barge Right.auto

@@ -4,28 +4,34 @@
     "type": "sequential",
     "data": {
       "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Center Barge to Reef"
-          }
-        },
         {
           "type": "named",
           "data": {
-            "name": "april-allign"
+            "name": "prepare-l4"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "Center Reef to Feed"
+            "pathName": "Center Barge to Reef 4 Left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 4 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
           }
         },
         {
@@ -37,25 +43,37 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Top Feed to Reef"
+            "pathName": "Bottom Feed to Reef 2"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "april-allign"
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "place-coral"
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
           }
         }
       ]
     }
   },
   "resetOdom": true,
-  "folder": null,
+  "folder": "2 Coral",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/3 Coral Cage 2.auto

@@ -0,0 +1,109 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 2 to Reef 5"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 5 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Feed to Reef 6"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Top Feed to Reef 6"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 6 to Top Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto

@@ -0,0 +1,121 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 5 to Reef"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 3 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/3 Coral Center Barge Bottom.auto

@@ -0,0 +1,109 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "prepare-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 4 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-right-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bottom Feed to Reef 2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "place-coral-left-l4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Reef 2 to Bottom Feed"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "grab-coral"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "feed-driveback"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Line-up-no-arm.auto

@@ -1,25 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Top Blue Cage to Reef"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral-left-l4"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/Mira's Auto.auto

@@ -1,61 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Top Blue Cage to Reef"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "april-allign"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Top Reef to Feed"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "grab-coral"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Top Feed to Reef"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "april-allign"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/New Auto.auto

@@ -7,49 +7,49 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Bottom Blue Cage to Reef"
+            "pathName": "Cage 1 to Reef"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "april-allign"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "Top Reef to Feed"
+            "pathName": "Cage 2 to Reef 5"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "grab-coral"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "Top Feed to Reef"
+            "pathName": "Cage 3 to Reef"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "april-allign"
+            "pathName": "Cage 4 to Reef"
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "place-coral"
+            "pathName": "Cage 5 to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Cage 6 to Reef"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Left"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Barge to Reef 4 Right"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Red Top Cage.auto

@@ -1,61 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Top Red Cage to Reef"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "april-allign"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Bottom Reef to Feed"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "grab-coral"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Bottom Feed to Reef"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "april-allign"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "place-coral"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/Taxi.auto

@@ -5,9 +5,9 @@
     "data": {
       "commands": [
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Taxi"
+            "name": "taxi"
           }
         }
       ]

src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3040625,
+        "y": 1.3559375
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.892340424329296,
+        "y": 2.067688275398428
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.5221875,
+        "y": 2.3675
+      },
+      "prevControl": {
+        "x": 2.589485733053583,
+        "y": 1.921600370835535
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 59.28109573597083
+  },
+  "reversed": false,
+  "folder": "Bottom Feed to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 53.426969021480645
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Cage 1 to Reef.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.344772727272727,
+        "x": 6.9834375,
-        "y": 2.6688636363636364
+        "y": 7.1084375
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.4715625,
+        "x": 6.292598684210526,
-        "y": 1.432386363636364
+        "y": 6.699835526315789
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.1823863636363636,
+        "x": 5.590213815789473,
-        "y": 1.026578282828283
+        "y": 5.622203947368421
       },
       "prevControl": {
-        "x": 0.9229528762543462,
+        "x": 5.846151315789473,
-        "y": 0.9200437072077402
+        "y": 6.024391447368421
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 55.05578949900953
+    "rotation": -118.81079374297312
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Barge to Reef",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 121.42956561483854
+    "rotation": -115.9743939624313
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path

@@ -0,0 +1,65 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.0078125,
+        "y": 6.0725
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.902067334527623,
+        "y": 5.78751784036735
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.590213815789473,
+        "y": 5.593338815789473
+      },
+      "prevControl": {
+        "x": 5.872115174798128,
+        "y": 6.170888760790882
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [
+    {
+      "fieldPosition": {
+        "x": 5.15,
+        "y": 4.85
+      },
+      "rotationOffset": 0.0,
+      "minWaypointRelativePos": 0.0,
+      "maxWaypointRelativePos": 1.0,
+      "name": "Point Towards Zone"
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -120.52970589993409
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -123.4533094540727
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Cage 3 to Reef.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.616287878787879,
+        "x": 6.9590625,
-        "y": 5.092234848484848
+        "y": 4.9878124999999995
       },
       "prevControl": null,
       "nextControl": {
-        "x": 8.269913357759823,
+        "x": 6.709257689548163,
-        "y": 5.035328772529629
+        "y": 4.997689578268469
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.370012626262627,
+        "x": 5.619078947368421,
-        "y": 5.346338383838384
+        "y": 5.593338815789473
       },
       "prevControl": {
-        "x": 4.997649306769947,
+        "x": 6.094391447368421,
-        "y": 5.369630479499745
+        "y": 5.5202138157894725
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,10 +45,10 @@
     "rotation": -121.26879518614867
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Barge to Reef",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 179.34296858150984
+    "rotation": -124.91940201245771
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Cage 4 to Reef.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.585795454545455,
+        "x": 6.97125,
-        "y": 1.9413510101010092
+        "y": 3.1474999999999995
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.96969696969697,
+        "x": 6.166875,
-        "y": 1.1282196969696958
+        "y": 2.8184375
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.319191919191919,
+        "x": 5.55172697368421,
-        "y": 2.6833333333333327
+        "y": 2.4470394736842103
       },
       "prevControl": {
-        "x": 5.75625,
+        "x": 6.3804769736842095,
-        "y": 2.256439393939394
+        "y": 2.7395394736842107
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 122.87136694597014
+    "rotation": 120.96375653207355
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Barge to Reef",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 179.5295681977034
+    "rotation": 118.9091836511478
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Cage 5 to Reef.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.016603535353535,
+        "y": 1.9718434343434341
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.670197826108406,
+        "y": 2.0396270358261894
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.5805921052631575,
+        "y": 2.4374177631578946
+      },
+      "prevControl": {
+        "x": 5.9030178398827875,
+        "y": 2.377301864673819
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.0,
+    "rotation": 119.99999999999999
+  },
+  "reversed": false,
+  "folder": "Barge to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 123.5866498700801
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Cage 6 to Reef.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.3145202020202018,
+        "x": 7.057260101010102,
-        "y": 0.9655934343434341
+        "y": 0.8537878787878788
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.39747159090909,
+        "x": 6.315277777777778,
-        "y": 1.3027556818181814
+        "y": 1.5144570707070704
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.3048863636363635,
+        "x": 5.561348684210526,
-        "y": 2.5292613636363637
+        "y": 2.456661184210526
       },
       "prevControl": {
-        "x": 5.893210227272728,
+        "x": 5.883902345826687,
-        "y": 1.9808238636363626
+        "y": 2.038842371079213
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 114.22774531795413
+    "rotation": 120.98817835541992
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Barge to Reef",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 53.446462811652175
+    "rotation": 132.5633517531898
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Left.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.565467171717171,
+        "x": 7.1296875,
-        "y": 4.045328282828282
+        "y": 4.0493749999999995
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.269289772727273,
+        "x": 6.833510101010102,
-        "y": 4.064886363636362
+        "y": 4.06893308080808
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.172414772727272,
+        "x": 6.3496875,
-        "y": 4.045328282828282
+        "y": 3.8421874999999996
       },
       "prevControl": {
-        "x": 5.696091638273898,
+        "x": 6.599301094934041,
-        "y": 4.0462802990619195
+        "y": 3.8282931405651617
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
     "rotation": -179.96096735022735
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Barge to Reef",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -179.2890804030095

src/main/deploy/pathplanner/paths/Center Barge to Reef 4 Right.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.116818181818182,
+        "x": 7.1053125,
-        "y": 6.956647727272727
+        "y": 4.061562499999999
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.123068181818182,
+        "x": 6.809135101010102,
-        "y": 6.275650252525252
+        "y": 4.081120580808079
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.6196875,
+        "x": 6.3496875,
-        "y": 5.35122159090909
+        "y": 4.195625
       },
       "prevControl": {
-        "x": 2.8392584714142375,
+        "x": 6.599301094934041,
-        "y": 5.792423901494171
+        "y": 4.181730640565162
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -59.239047023115106
+    "rotation": -179.96096735022735
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Barge to Reef",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -52.93323259086456
+    "rotation": -179.2890804030095
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path

@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 7.575631313127491,
-        "y": 6.139141414136251
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 6.3210457316672,
-        "y": 6.004686892287159
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.268371212121212,
-        "y": 5.417487373737374
-      },
-      "prevControl": {
-        "x": 6.306061170284897,
-        "y": 5.670117895431184
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -120.86136963407526
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": -179.99261564513898
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/paths/Center Reef to Feed.path

@@ -1,70 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 5.982954545454545,
-        "y": 4.03497159090909
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 7.109744318181819,
-        "y": 3.8355397727272718
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.232244318181818,
-        "y": 5.281420454545455
-      },
-      "prevControl": {
-        "x": 7.1397939460232385,
-        "y": 4.676387369317844
-      },
-      "nextControl": {
-        "x": 5.574119318181818,
-        "y": 5.720170454545454
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.121401515151515,
-        "y": 7.003093434343434
-      },
-      "prevControl": {
-        "x": 0.32627122293847843,
-        "y": 7.229156472141377
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": -53.93780932479241
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 179.1574757392596
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/paths/New Path.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.575631313131314,
+        "x": 4.0,
-        "y": 7.287689393939393
+        "y": 6.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.599873737373737,
+        "x": 4.2495147804709745,
-        "y": 8.029671717171716
+        "y": 5.984431624152738
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.344772727272727,
+        "x": 4.0,
-        "y": 5.391107954545454
+        "y": 6.0
       },
       "prevControl": {
-        "x": 5.121161616161616,
+        "x": 4.249520880858856,
-        "y": 5.00487058080808
+        "y": 5.984529705386749
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -122.99770510121631
+    "rotation": 0.0
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5221875,
+        "y": 2.4040625
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.9564891755960816,
+        "y": 2.1114702488693777
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3284375,
+        "y": 1.3803125
+      },
+      "prevControl": {
+        "x": 2.0502196323692035,
+        "y": 1.7292616486680126
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.5,
+    "maxAcceleration": 5.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 55.37584492005105
+  },
+  "reversed": false,
+  "folder": "Reef to Bottom Feed",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 58.10920819815426
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.426644736842105,
+        "y": 2.851151315789474
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.23151774045974,
+        "y": 2.4081021765718877
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.7415296052631577,
+        "y": 1.5618421052631584
+      },
+      "prevControl": {
+        "x": 3.369389020277641,
+        "y": 1.8741228567589594
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 55.05578949900953
+  },
+  "reversed": false,
+  "folder": "Reef to Bottom Feed",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 121.42956561483854
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.092642045454545,
+        "x": 5.9750822368421055,
-        "y": 3.9651704545454542
+        "y": 3.9865131578947364
       },
       "prevControl": null,
       "nextControl": {
-        "x": 9.024289772727268,
+        "x": 7.0046052631578934,
-        "y": 3.446647727272728
+        "y": 1.6388157894736843
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.3755050505050503,
+        "x": 1.3040625,
-        "y": 0.8842803030303026
+        "y": 1.368125
       },
       "prevControl": {
-        "x": 1.5707222528658078,
+        "x": 1.7992598684210526,
-        "y": 1.040454064918909
+        "y": 2.302713815789474
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 5.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 5.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 0.0,
     "rotation": 52.857102599919905
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Reef to Bottom Feed",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -179.46454101443553
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.565467171717171,
+        "x": 5.946217105263157,
-        "y": 0.8334595959595947
+        "y": 4.178947368421053
       },
       "prevControl": null,
       "nextControl": {
-        "x": 8.123574009981496,
+        "x": 6.604342105263157,
-        "y": 0.3592362071562901
+        "y": 6.518947368421053
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.3903409090909085,
+        "x": 1.452878289473684,
-        "y": 2.6833333333333327
+        "y": 6.565131578947369
       },
       "prevControl": {
-        "x": 5.06636861749931,
+        "x": 2.0499142743221688,
-        "y": 2.9361778277713553
+        "y": 5.561413144603935
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -57.4259428654274
+    "rotation": -53.93780932479242
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Reef to Top Feed",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -0.9548412538721401
+    "rotation": 178.9390883097358
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.284943181818181,
+        "x": 5.41702302631579,
-        "y": 5.401079545454545
+        "y": 5.256578947368421
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.844554924242423,
+        "x": 4.4044191919191915,
-        "y": 6.744466540404039
+        "y": 5.9460227272727275
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.2230429292929292,
+        "x": 1.5076388888888888,
-        "y": 7.003093434343434
+        "y": 6.484722222222222
       },
       "prevControl": {
-        "x": 1.9644034090909093,
+        "x": 3.184818220656944,
-        "y": 5.750085227272726
+        "y": 6.004734862812045
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 5.5,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 5.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -45,10 +45,10 @@
     "rotation": -54.83470564502973
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Reef to Top Feed",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -121.0370223454415
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5099747474747476,
+        "y": 5.742739898989899
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.36577546257746,
+        "y": 6.2120596924932325
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4143914473684207,
+        "y": 6.584375
+      },
+      "prevControl": {
+        "x": 2.7066049155713703,
+        "y": 6.056626627340928
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.5,
+    "maxAcceleration": 5.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.93780932479242
+  },
+  "reversed": false,
+  "folder": "Reef to Top Feed",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -58.96173664449721
+  },
+  "useDefaultConstraints": false
+}

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

@@ -1,70 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 7.565467171717171,
-        "y": 7.348674242424242
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 7.8906231049309685,
-        "y": 7.339915637757156
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.305113636363636,
-        "y": 7.348674242424242
-      },
-      "prevControl": {
-        "x": 5.928529040722133,
-        "y": 7.353679406601922
-      },
-      "nextControl": {
-        "x": 6.681698232005139,
-        "y": 7.343669078246562
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 3.8889204545454543,
-        "y": 7.348674242424242
-      },
-      "prevControl": {
-        "x": 4.138853417354843,
-        "y": 7.3544633789160185
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 179.59913485447024
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": -179.85896618052556
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/paths/Top Feed to Reef 6.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.31625,
+        "y": 6.730625
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.5451175614850996,
+        "y": 6.277541643530879
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.540467171717172,
+        "y": 5.7630681818181815
+      },
+      "prevControl": {
+        "x": 2.590802656557517,
+        "y": 6.105344244914677
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.239047023115106
+  },
+  "reversed": false,
+  "folder": "Top Feed to Reef",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -52.93323259086456
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path

@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 7.595959595959596,
-        "y": 3.0289141414141416
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 7.319147727272727,
-        "y": 2.3198579545454536
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.349684343434343,
-        "y": 2.6934974747474745
-      },
-      "prevControl": {
-        "x": 6.401761363636364,
-        "y": 1.5819602272727276
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 123.49004753500587
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": -178.9832812445108
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/settings.json

@@ -2,18 +2,28 @@
   "robotWidth": 0.9,
   "robotLength": 0.9,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
-  "autoFolders": [],
+    "Barge to Reef",
+    "Bottom Feed to Reef",
+    "Reef to Top Feed",
+    "Reef to Bottom Feed",
+    "Top Feed to Reef"
+  ],
+  "autoFolders": [
+    "1 Coral",
+    "2 Coral",
+    "3 Coral"
+  ],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 74.088,
+  "robotMass": 58.18,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
+  "driveWheelRadius": 0.0508,
-  "driveGearing": 5.143,
+  "driveGearing": 6.12,
   "maxDriveSpeed": 5.45,
   "driveMotorType": "krakenX60",
   "driveCurrentLimit": 60.0,

src/main/java/frc4388/robot/Constants.java

@@ -21,7 +21,6 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
@@ -55,6 +54,7 @@ import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.ReefPositionHelper;
 import frc4388.utility.Trim;
+import frc4388.utility.configurable.ConfigurableDouble;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -69,7 +69,7 @@ public final class Constants {
     
     public static final class SwerveDriveConstants {
 
-        public static final double MAX_ROT_SPEED        = 3.5;
+        public static final double MAX_ROT_SPEED        = Math.PI * 2;
         public static final double AUTO_MAX_ROT_SPEED = 1.5;
         public static final double MIN_ROT_SPEED        = 1.0;
         public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
@@ -114,7 +114,7 @@ public final class Constants {
 
         private static final class ModuleSpecificConstants { //2025
             //Front Left
-            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.4794921875+0.25);
+            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
             private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
@@ -122,7 +122,7 @@ public final class Constants {
             private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
             
             //Front Right
-            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.5);
+            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
             private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
             private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
@@ -130,7 +130,7 @@ public final class Constants {
             private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
 
             //Back Left
-            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(-0.867431640625+0.25);
+            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5);
             private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
@@ -138,7 +138,7 @@ public final class Constants {
             private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
             
             //Back Right
-            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.425537109375-0.25+0.25);
+            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5);
             private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
             private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
             private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
@@ -146,40 +146,6 @@ public final class Constants {
             private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
         }
 
-        /* private static final class ModuleSpecificConstants { // 2024
-            //Front Left
-            private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125);
-            private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
-            private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
-            private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
-            private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
-            private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
-            
-            //Front Right
-            private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625);
-            private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
-            private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
-            private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
-            private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
-            private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
-
-            //Back Left
-            private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5);
-            private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
-            private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
-            private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
-            private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
-            private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
-            
-            //Back Right
-            private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5);
-            private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
-            private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
-            private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
-            private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
-            private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
-        } */
-
         public static final class IDs {
             public static final CanDevice RIGHT_FRONT_WHEEL   = new CanDevice("RIGHT_FRONT_WHEEL", 4);
             public static final CanDevice RIGHT_FRONT_STEER   = new CanDevice("RIGHT_FRONT_STEER", 5);
@@ -228,7 +194,7 @@ public final class Constants {
                 .withKS(0).withKV(0.124);
             
             public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
-            public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST
+            public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
         }
 
         public static final class Configurations {
@@ -322,17 +288,29 @@ public final class Constants {
       }
     
     public static final class LiDARConstants {
+        public static final int REEF_LIDAR_DIO_CHANNEL = 7;
+        public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
+
+        public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
+
         public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
-        public static final int LIDAR_DIO_CHANNEL = 7;
         public static final int LIDAR_MICROS_TO_CM = 10;
         public static final int SECONDS_TO_MICROS = 1000000;
     }
 
     public static final class AutoConstants {
-        public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
+        // public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
-        public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
+        public static final Gains XY_GAINS = new Gains(8,0,0.0);
+        // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
+        // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
+        // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
+       // public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
+
+        // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
+        // public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
 
         public static final Trim X_OFFSET_TRIM =        new Trim("X Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
+        // public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5);
         public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
         public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
         public static final Trim ARM_OFFSET_TRIM =      new Trim("ARM Offset Trim",      -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
@@ -340,25 +318,46 @@ public final class Constants {
         public static final double XY_TOLERANCE = 0.07; // Meters
         public static final double ROT_TOLERANCE = 5; // Degrees
 
+        public static final double MIN_XY_PID_OUTPUT = 0.0;
+        public static final double MIN_ROT_PID_OUTPUT = 0.0;
+
+        public static final double VELOCITY_THRESHHOLD = 0.01;
+                
         // X is tangent to reef side
         // Y is normal to reef side
-        public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field
+        public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
-        public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
+        public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
+        public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
 
-        public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
+        public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
-        public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
+        // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
         
-        public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
+        public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
-        public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
         
-        public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
+        public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
+        public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2);
 
-        public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
+        public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
+        
+        // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
+        // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
+        
+        // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
+        
+        // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
+
+        // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
 
         public static final int L4_DRIVE_TIME = 250; //Milliseconds
         // public static final int L3_DRIVE_TIME = 500;
         public static final int L2_DRIVE_TIME = 250; //Milliseconds
         public static final int ALGAE_DRIVE_TIME = 500;
+        public static final double STOP_VELOCITY = 0.0;
     }
 
     public static final class VisionConstants { 
@@ -368,14 +367,14 @@ public final class Constants {
         public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
         public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
         
-        public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters
+        public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
 
         // Photonvision thing
         // The standard deviations of our vision estimated poses, which affect correction rate
         // X, Y, Theta
         // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
-        public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+        public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
-        public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
+        public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
     }
 
     public static final class FieldConstants {
@@ -403,6 +402,7 @@ public final class Constants {
         public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
         public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
         public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
+        public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
 
         public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
         public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
@@ -416,39 +416,47 @@ public final class Constants {
         public static final double LEFT_AXIS_DEADBAND = 0.1;
 
     }
-    public static final class ClimberConstants {
+
-        public static final CanDevice CLIMBER_ID = new CanDevice("Climber", 311);
-        public static final double CLIMBER_SPEED = 0.1;
-    }
     public static final class ElevatorConstants {
         public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
         public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
 
+        public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75;
+        public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20;
+
+        // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
+        
         public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
         public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
+        public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND
+
         
         public static final double GEAR_RATIO_ELEVATOR = -9.0;
         //Max for elevator = 50%
         
         public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
         public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
+        public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
         public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
         public static final double SCORING_THREE_ELEVATOR = -9.25;
-        public static final double DEALGAE_L2_ELEVATOR = -23.5;
+        public static final double DEALGAE_L2_ELEVATOR = -4;
-        public static final double DEALGAE_L3_ELEVATOR = -33.75;
+        public static final double DEALGAE_L3_ELEVATOR = -26.5;
         public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
         
         public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
         public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
         //Max for endefector = 60%
+        public static final double L2_SCORE_ELEVATOR = -5;
+        public static final double L2_LEAVE_ELEVATOR = -11;
 
         public static final double L2_SCORE_ENDEFFECTOR = -19;
 
         public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
-        public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
+        public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
         public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
         public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
-        public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
+        public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
+        public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
         public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
 
         public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()

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

@@ -14,6 +14,7 @@ import java.io.File;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.function.BooleanSupplier;
+import java.util.function.Supplier;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -27,6 +28,7 @@ import frc4388.utility.controller.XboxController;
 import frc4388.utility.controller.ButtonBox;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.robot.Constants.FieldConstants;
+import frc4388.robot.Constants.LiDARConstants;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.Constants.AutoConstants;
@@ -36,6 +38,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 // Commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -43,12 +47,16 @@ import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 // Autos
 import frc4388.utility.controller.VirtualController;
+import frc4388.robot.commands.DriveUntilLiDAR;
 import frc4388.robot.commands.GotoLastApril;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.robot.commands.MoveForTimeCommand;
+import frc4388.robot.commands.MoveUntilSuply;
+import frc4388.robot.commands.WhileTrueCommand;
 import frc4388.robot.commands.waitElevatorRefrence;
 import frc4388.robot.commands.waitEndefectorRefrence;
 import frc4388.robot.commands.waitFeedCoral;
+import frc4388.robot.commands.waitSupplier;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
 
@@ -61,7 +69,6 @@ import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.Elevator.CoordinationState;
 import frc4388.robot.subsystems.Lidar;
-import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Elevator;
 // import frc4388.robot.subsystems.Endeffector;
 import frc4388.robot.subsystems.SwerveDrive;
@@ -84,15 +91,16 @@ import frc4388.utility.configurable.ConfigurableString;
  */
 public class RobotContainer {
     /* RobotMap */
+    
     public final RobotMap m_robotMap = new RobotMap();
     
     /* Subsystems */
     public final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
-    public final Lidar m_lidar = new Lidar();
+    public final Lidar m_reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
-    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED);
+    public final Lidar m_reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
+    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
-    public final Climber m_robotClimber = new Climber(m_robotMap.climber);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
 
@@ -110,8 +118,9 @@ public class RobotContainer {
 
     // ! Teleop Commands
     public void stop() {
-        new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;;
+        new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
         m_robotSwerveDrive.stopModules();
+        Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
     }
 
     // ! /*  Autos */
@@ -119,18 +128,23 @@ public class RobotContainer {
     private SendableChooser<String> autoChooser;
     private Command autoCommand;
 
+    private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove);
+    // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed);
+    // private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed);
+    private Command waitDebuger = new waitSupplier(() -> true);
+
     // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
     // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, 
     // () -> autoplaybackName.get(), // lastAutoName
     //        new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
     //        true, false);
     // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
-    private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
+    private Command AprilLidarAlignL4RightFullAuto = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT)
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
         ), () -> m_robotElevator.isL4Primed()),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -139,8 +153,18 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        waitDebuger.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
+        new ParallelRaceGroup(
+            new WaitCommand(1),
+            new MoveUntilSuply(
+                m_robotSwerveDrive, 
+                new Translation2d(0,-0.5), 
+                new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
+        ),
+        new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
 
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
 
@@ -149,7 +173,91 @@ public class RobotContainer {
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
 
+        // new ConditionalCommand(
+        //     new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+            //  () -> m_robotElevator.hasCoral())
+
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
+    );
+
+    private Command AprilLidarAlignL4RightSemiAuto = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+        
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
+        ), () -> m_robotElevator.isL4Primed()),
+
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
+
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
+        waitDebuger.asProxy(),
+        // new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        // waitDebuger.asProxy(),
+        // new ParallelRaceGroup(
+        //     new WaitCommand(1),
+        //     new MoveUntilSuply(
+        //         m_robotSwerveDrive, 
+        //         new Translation2d(0,-0.5), 
+        //         new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
+        // ),
+        // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
+            //  () -> m_robotElevator.hasCoral())
+
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
+    );
+    
+    private Command WannaSeeMeDunk = new SequentialCommandGroup(
+        new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive),
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+        new waitEndefectorRefrence(m_robotElevator),
+        new MoveForTimeCommand(
+            m_robotSwerveDrive, 
+            new Translation2d(0,1), 
+            new Translation2d(), 
+            AutoConstants.L4_DRIVE_TIME, 
+            true
+        ),
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+        new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive)
+    );
+
+    /* private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+        
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
+        ), () -> m_robotElevator.isL4Primed()),
+
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
+
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
+        waitDebuger.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
+
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+
+        new waitEndefectorRefrence(m_robotElevator),
+
+        new MoveForTimeCommand(m_robotSwerveDrive, 
+        new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
+
+        new ConditionalCommand(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+             () -> m_robotElevator.hasCoral()),
+
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
@@ -160,7 +268,7 @@ public class RobotContainer {
 
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT)
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
         ), () -> m_robotElevator.isL4Primed()),
         
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -169,19 +277,118 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
-
+        waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
 
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
 
         new waitEndefectorRefrence(m_robotElevator),
 
+        
         new MoveForTimeCommand(m_robotSwerveDrive, 
             new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
 
+        new ConditionalCommand(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+                () -> m_robotElevator.hasCoral()),
+
+
+
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
+    ); */
+
+    private Command AprilLidarAlignL4LeftFullAuto = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+
+        // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
+
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
+        ), () -> m_robotElevator.isL4Primed()),
+        
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
+        waitDebuger.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
+        new ParallelRaceGroup(
+            new WaitCommand(1),
+            new MoveUntilSuply(
+                m_robotSwerveDrive, 
+                new Translation2d(0,-0.5), 
+                new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
+        ),
+        new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
+        
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+
+        new waitEndefectorRefrence(m_robotElevator),
+
+        
+        new MoveForTimeCommand(m_robotSwerveDrive, 
+            new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
+
+        // // new ConditionalCommand(
+        // //     new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+                // () -> m_robotElevator.hasCoral()),
+
+
+
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
+    );
+
+    private Command AprilLidarAlignL4LeftSemiAuto = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+
+        // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
+
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
+        ), () -> m_robotElevator.isL4Primed()),
+        
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
+        waitDebuger.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        // waitDebuger.asProxy(),
+        // new ParallelRaceGroup(
+        //     new WaitCommand(1),
+        //     new MoveUntilSuply(
+        //         m_robotSwerveDrive, 
+        //         new Translation2d(0,-0.5), 
+        //         new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
+        // ),
+        // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
+        
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+
+        // new waitEndefectorRefrence(m_robotElevator),
+
+        
+        // new MoveForTimeCommand(m_robotSwerveDrive, 
+        //     new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
+
+        // // // new ConditionalCommand(
+        // // //     new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        //     new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+        //         // () -> m_robotElevator.hasCoral()),
 
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
@@ -191,14 +398,16 @@ public class RobotContainer {
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
         // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT)
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true)
         ), () -> m_robotElevator.isL3Primed()),
 
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true),
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        waitDebuger.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -214,7 +423,7 @@ public class RobotContainer {
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
         // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT)
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true)
         ),() -> m_robotElevator.isL3Primed()),
         
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
@@ -223,9 +432,11 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true),
+        waitDebuger.asProxy(),
         
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
 
@@ -239,12 +450,18 @@ public class RobotContainer {
     
     private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true),
+        waitDebuger.asProxy(),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true),
+        waitDebuger.asProxy(),
         
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
@@ -254,75 +471,147 @@ public class RobotContainer {
 
     private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true),
+        waitDebuger.asProxy(),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true),
+        waitDebuger.asProxy(),
 
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2ScoreLeave);}, m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
-    private Command leftAlgaeRemove = new SequentialCommandGroup(
+    // private Command leftAlgaeRemove = new SequentialCommandGroup(
+    //     new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+    //     new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
+    //     new waitEndefectorRefrence(m_robotElevator),
+    //     new waitElevatorRefrence(m_robotElevator),
+    //     new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
+    //     new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+    //     new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
+    //     new MoveForTimeCommand(m_robotSwerveDrive, 
+    //     new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
+    //     new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
+    //     new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
+    // );
+
+    private Command lowerAlgaeRemove = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        waitDebuger.asProxy(),
-        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
+        // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
+            new Translation2d(1,0), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME / 2, true),
+        new MoveForTimeCommand(m_robotSwerveDrive, 
+            new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME * 2, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
-    private Command rightAlgaeRemove = new SequentialCommandGroup(
+    // private Command rightAlgaeRemove = new SequentialCommandGroup(
+    //     new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+    //     new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
+    //     new waitEndefectorRefrence(m_robotElevator),
+    //     new waitElevatorRefrence(m_robotElevator),
+    //     new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
+    //     new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+    //     new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
+    //     new MoveForTimeCommand(m_robotSwerveDrive, 
+    //     new Translation2d(1,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
+    //     new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
+    //     new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
+    // );
+    private Command upperAlgaeRemove = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
-        new LidarAlign(m_robotSwerveDrive, m_lidar),
+        waitDebuger.asProxy(),
-        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
+        // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
+            new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
     private Command thrustIntake = new SequentialCommandGroup(
-        new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 100, true),
+        new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod(), m_robotSwerveDrive),
-        new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)
+        new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 300, true),
+        new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive),
+        new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod(), m_robotSwerveDrive)
     );
     
     private Boolean operatorManualMode = false;
     
+    // public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left.asProxy(), () -> !m_robotElevator.hasCoral());
+    // public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right.asProxy(), () -> !m_robotElevator.hasCoral());
+    // public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left.asProxy(), () -> !m_robotElevator.hasCoral());
+    // public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right.asProxy(), () -> !m_robotElevator.hasCoral());
+    // public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left.asProxy(), () -> !m_robotElevator.hasCoral());
+    // public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right.asProxy(), () -> !m_robotElevator.hasCoral());
+    
+    // public Command LoopAprilLidarAlignL4Left = new SequentialCommandGroup(AprilLidarAlignL4Left.asProxy(), new ConditionalCommand(AprilLidarAlignL4Left.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral()));
+    // public Command LoopAprilLidarAlignL4Right = new SequentialCommandGroup(AprilLidarAlignL4Right.asProxy(), new ConditionalCommand(AprilLidarAlignL4Right.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral()));
+    // public Command LoopAprilLidarAlignL3Left = new SequentialCommandGroup(AprilLidarAlignL3Left.asProxy(), new ConditionalCommand(AprilLidarAlignL3Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
+    // public Command LoopAprilLidarAlignL3Right = new SequentialCommandGroup(AprilLidarAlignL3Right.asProxy(), new ConditionalCommand(AprilLidarAlignL3Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
+    // public Command LoopAprilLidarAlignL2Left = new SequentialCommandGroup(AprilLidarAlignL2Left.asProxy(), new ConditionalCommand(AprilLidarAlignL2Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
+    // public Command LoopAprilLidarAlignL2Right = new SequentialCommandGroup(AprilLidarAlignL2Right.asProxy(), new ConditionalCommand(AprilLidarAlignL2Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
+        NamedCommands.registerCommand("taxi", new SequentialCommandGroup(
+            new MoveForTimeCommand(m_robotSwerveDrive, 
+                new Translation2d(0, -1), 
+                new Translation2d(), 1000, true
+        ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
+
+        NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy());
+        NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
+
+        NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, 
+        new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
+        // NamedCommands.registerCommand("feed-driveback", Commands.none());
+        NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
 
-        NamedCommands.registerCommand("grab-coral", new waitFeedCoral(m_robotElevator));
         NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
             new MoveForTimeCommand(m_robotSwerveDrive, 
                 new Translation2d(0, 1), 
-                new Translation2d(), 200, true
+                new Translation2d(), 300, true
-        ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
                 
-        NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
+        ), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
-        NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
+        new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
+        
+        NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4LeftFullAuto);
+        NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4RightFullAuto);
         NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
         NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
         NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
         NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
 
-        NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
+
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT),
+        NamedCommands.registerCommand("lower-algae-removal", lowerAlgaeRemove);
-            new LidarAlign(m_robotSwerveDrive, m_lidar)
+        NamedCommands.registerCommand("upper-algae-removal", upperAlgaeRemove);
+
+        NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup(
+            // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)),
+            // new waitElevatorRefrence(m_robotElevator),
+            // new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)),
+            new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour))
         ));
 
         configureButtonBindings();        
@@ -417,10 +706,10 @@ public class RobotContainer {
 
         // While Left Trigger Pressed: Trims
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 0 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
-            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
+            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
 
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 180  && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
-            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
+            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
 
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 90 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
             .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
@@ -428,6 +717,14 @@ public class RobotContainer {
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 270 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
             .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
         
+        new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
+            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;}))
+            .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;}));
+
+        new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod()))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod()));
+        
         // While Left Trigger NOT Pressed: Fine Alignment
         new Trigger(() -> getDeadbandedDriverController().getPOV() != -1 && !(getDeadbandedDriverController().getLeftTriggerAxis() > 0.8))
             .whileTrue(new RunCommand(
@@ -440,11 +737,16 @@ public class RobotContainer {
                 ), m_robotSwerveDrive))
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
         
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+        //        .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, 
+        //        new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
+            .onTrue(WannaSeeMeDunk.asProxy());
+
         new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
             .onTrue(thrustIntake.asProxy());
           
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); 
+            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar)); 
         
             
         // ?  /* Operator Buttons */
@@ -457,10 +759,10 @@ public class RobotContainer {
         // Button box
 
         new JoystickButton(getButtonBox(), ButtonBox.Five)
-            .onTrue(AprilLidarAlignL4Left);
+            .onTrue(AprilLidarAlignL4LeftSemiAuto);
         
         new JoystickButton(getButtonBox(), ButtonBox.One)
-            .onTrue(AprilLidarAlignL4Right);
+            .onTrue(AprilLidarAlignL4RightSemiAuto);
 
         new JoystickButton(getButtonBox(), ButtonBox.Six)
             .onTrue(AprilLidarAlignL3Left);
@@ -476,11 +778,11 @@ public class RobotContainer {
     
         // Lower algae removal
         new JoystickButton(getButtonBox(), ButtonBox.Eight)
-            .onTrue(leftAlgaeRemove);
+            .onTrue(lowerAlgaeRemove);
         
         // Upper algae removal
         new JoystickButton(getButtonBox(), ButtonBox.Four)
-            .onTrue(rightAlgaeRemove);
+            .onTrue(upperAlgaeRemove);
             
 
         // Cancel button
@@ -511,18 +813,18 @@ public class RobotContainer {
             .onTrue (new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed), m_robotElevator))
             .onFalse(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go), m_robotElevator));
         
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
             .onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;}));
         
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking()));
+        //     .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking()));
 
         // Auto Scoring 
         new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
-            .onTrue(AprilLidarAlignL4Left);
+            .onTrue(AprilLidarAlignL4LeftSemiAuto);
         
         new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
-            .onTrue(AprilLidarAlignL4Right);
+            .onTrue(AprilLidarAlignL4RightSemiAuto);
 
         new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode)
             .onTrue(AprilLidarAlignL3Left);
@@ -538,19 +840,11 @@ public class RobotContainer {
 
         //Controller Lower Algae Removal
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180 && !operatorManualMode)
-            .onTrue(leftAlgaeRemove);
+            .onTrue(lowerAlgaeRemove);
 
         //Controller Upper Algae Removal
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && !operatorManualMode)
-            .onTrue(rightAlgaeRemove);
+            .onTrue(upperAlgaeRemove);
-
-        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
-            .onTrue(new InstantCommand(() -> m_robotClimber.climberOut()))
-            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimber()));
-
-        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
-            .onTrue(new InstantCommand(() -> m_robotClimber.climberIn()))
-            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimber()));
 
         // ? /* Programer Buttons (Controller 3)*/
 
@@ -627,6 +921,8 @@ public class RobotContainer {
         // File dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
         String[] autos = dir.list();
 
+        if(autos == null) return;
+
         for (String auto : autos) {
             if (auto.endsWith(".auto"))
                 autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
@@ -634,7 +930,15 @@ public class RobotContainer {
         }
 
         autoChooser.onChange((filename) -> {
-            autoCommand = new PathPlannerAuto(filename);
+            if (filename.equals("Taxi")) {
+                autoCommand = new SequentialCommandGroup(
+                    new MoveForTimeCommand(m_robotSwerveDrive, 
+                        new Translation2d(0, -1), 
+                        new Translation2d(), 1000, true
+                ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
+            } else {
+                autoCommand = new PathPlannerAuto(filename);
+            }
             System.out.println("Robot Auto Changed " + filename);
         });
         // SmartDashboard.putData(autoChooser);

src/main/java/frc4388/robot/RobotMap.java

@@ -7,16 +7,25 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix6.hardware.TalonFX;
+
 import org.photonvision.PhotonCamera;
 
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 
 import edu.wpi.first.wpilibj.DigitalInput;
-import frc4388.robot.Constants.ClimberConstants;
 import frc4388.robot.Constants.ElevatorConstants;
+// import edu.wpi.first.wpilibj.motorcontrol.Spark;
+// import frc4388.robot.Constants.LEDConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.Constants.VisionConstants;
+// import frc4388.robot.subsystems.SwerveModule;
+import frc4388.utility.RobotGyro;
 
 /**
  * Defines and holds all I/O objects on the Roborio. This is useful for unit
@@ -47,12 +56,13 @@ public class RobotMap {
     public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
     public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
     
+
     public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
     public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
+    public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
 
-    /* Climber Subsytem */
+    void configureDriveMotorControllers() {
-    public final TalonFX climber = new TalonFX(ClimberConstants.CLIMBER_ID.id);
+        // endeffector.saf
-
+    }
-    void configureDriveMotorControllers() {}
         
 }

src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java

@@ -0,0 +1,54 @@
+package frc4388.robot.commands;
+
+import java.time.Instant;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Lidar;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.TimesNegativeOne;
+
+// Command to repeat a joystick movement for a specific time.
+public class DriveUntilLiDAR extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final Lidar m_lidar;
+    private final double mindistance;
+    private final boolean robotRelative;
+
+    public DriveUntilLiDAR(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        Lidar lidar,
+        double mindistance,
+        boolean robotRelative) {
+            addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.m_lidar = lidar;
+        this.mindistance = mindistance;
+        this.robotRelative = robotRelative;
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveFine(leftStick, rightStick, 0.3);
+    }
+
+    @Override
+    public boolean isFinished() {
+        if (Math.abs(m_lidar.getDistance()) < mindistance) {
+            swerveDrive.softStop();
+            return true;
+        }
+        return false;
+    }
+}

src/main/java/frc4388/robot/commands/GotoLastApril.java

@@ -1,5 +1,7 @@
 package frc4388.robot.commands;
 
+import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
+
 import java.util.Optional;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -29,25 +31,27 @@ public class GotoLastApril extends Command {
 
     private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
     private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
-    private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
+    // private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
     private Pose2d targetpos;
 
     SwerveDrive swerveDrive;
     Vision vision;
     double distance;
     Side side;
+    boolean waitVelocityZero;
 
     /**
      * Command to drive robot to position.
      * @param SwerveDrive m_robotSwerveDrive
      */
 
-    public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side) {
+    public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
         this.swerveDrive = swerveDrive;
         this.vision = vision;
         this.distance = distance;
         this.side = side;
-        // addRequirements(swerveDrive);
+        this.waitVelocityZero = waitVelocityZero && false;
+        addRequirements(swerveDrive);
     }
 
 
@@ -56,24 +60,43 @@ public class GotoLastApril extends Command {
         tagRelativeXError = val;
     }
 
-    Alliance alliance = null;
-
     @Override
     public void initialize() {
+        // double kP = AutoConstants.P_XY_GAINS.get();
+        // double kI = AutoConstants.I_XY_GAINS.get();
+        // double kD = AutoConstants.D_XY_GAINS.get();
+        // xPID = new PID(new Gains(
+        //     kP, 
+        //     kI, 
+        //     kD), 
+        // 0);
+        // yPID = new PID(new Gains(
+        //     kP, 
+        //     kI, 
+        //     kD),
+        // 0);
+
+        // System.out.println("kP: "+kP);
+        // System.out.println("kI: "+kI);
+        // System.out.println("kD: "+kD);
         xPID.initialize();
         yPID.initialize();
-        this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), side, 
+        this.targetpos = ReefPositionHelper.getNearestPosition(
-        Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), 
+            this.vision.getPose2d(), 
-        distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get()));
+            side, 
-        Optional<Alliance> a = DriverStation.getAlliance();
+            Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), 
-        if(!a.isEmpty())
+            distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
-            alliance = a.get();
+        );
     }
     
     double xerr;
     double yerr;
     double roterr;
 
+    double xoutput;
+    double youtput;
+    double rotoutput;
+
     @Override
     public void execute() {
         xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
@@ -85,28 +108,27 @@ public class GotoLastApril extends Command {
 
         roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
 
-        boolean invert = Math.abs(roterr) > 180;
-
         if(roterr > 180){
             roterr -= 360;
         }else if(roterr < -180){
             roterr += 360;
         }
 
-        SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
+        // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
-        SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
+        // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
-        SmartDashboard.putNumber("Rotational PID error", roterr);
+        // SmartDashboard.putNumber("Rotational PID error", roterr);
 
-        // SmartDashboard.putNumber("PID X Error", xerr);
+        SmartDashboard.putNumber("PID X Error", xerr);
-        // SmartDashboard.putNumber("PID Y Error", yerr);
+        SmartDashboard.putNumber("PID Y Error", yerr);
+        SmartDashboard.putNumber("PID Rot Error", roterr);
 
-        double xoutput = xPID.update(xerr);
+        xoutput = xPID.update(xerr);
-        double youtput = yPID.update(yerr);
+        youtput = yPID.update(yerr);
-        double rotoutput = rotPID.update(roterr);
+        // rotoutput = rotPID.update(roterr);
 
         xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
         youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
-        rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
+        // rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
         
 
 
@@ -116,24 +138,28 @@ public class GotoLastApril extends Command {
             // 0,0
         );
 
-        Translation2d rightStick = new Translation2d(
+        // Translation2d rightStick = new Translation2d(
-            Math.max(Math.min(rotoutput, 1), -1), 
+        //     Math.max(Math.min(rotoutput, 1), -1), 
-           0
+        //    0
-        );
+        // );
-
-
 
         setTagRelativeXError(
             new Translation2d(xerr, yerr).getAngle()
             .rotateBy(targetpos.getRotation())
             .getCos());
 
-        swerveDrive.driveWithInput(leftStick, rightStick, true);
+        swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
+        // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
     }
 
     @Override
     public final boolean isFinished() {
-        boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
+        boolean finished = (
+            (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
+            (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
+        );
         // System.out.println(finished);
 
         if(finished)

src/main/java/frc4388/robot/commands/LidarAlign.java

@@ -41,7 +41,7 @@ public class LidarAlign extends Command {
     this.currentFinderTick = 0;
     this.speed = 0.4; // TODO: find good speed for this
     this.foundReef = false;
-    this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
+    this.headedRight = (GotoLastApril.tagRelativeXError < 0);
     this.additionalDistance = 0;
     this.bounces = 0;
   }
@@ -97,7 +97,7 @@ public class LidarAlign extends Command {
       currentFinderTick = 0;
       foundReef = false;
       return false;
-    } else if (bounces == 2) {
+    } else if (bounces >= 3) {
       swerveDrive.stopModules();
       return true;
     } else {

src/main/java/frc4388/robot/commands/MoveUntilSuply.java

@@ -0,0 +1,47 @@
+package frc4388.robot.commands;
+
+import java.time.Instant;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.TimesNegativeOne;
+
+// Command to repeat a joystick movement for a specific time.
+public class MoveUntilSuply extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final Supplier<Boolean> truth;
+    private final boolean robotRelative;
+
+    public MoveUntilSuply(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        Supplier<Boolean> truth,
+        boolean robotRelative) {
+            addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.truth = truth;
+        this.robotRelative = robotRelative;
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return truth.get();
+    }
+}

src/main/java/frc4388/robot/commands/WhileTrueCommand.java

@@ -0,0 +1,105 @@
+package frc4388.robot.commands;
+
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+import java.util.function.BooleanSupplier;
+
+/**
+ * A command composition that runs one of two commands, depending on the value of the given
+ * condition when this command is initialized.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class WhileTrueCommand extends Command {
+  private final Command m_whileTrue;
+  private final BooleanSupplier m_condition;
+
+  /**
+   * Creates a new WhileTrueCommand.
+   *
+   * @param whileTrue the command to run while the condition is true
+   * @param condition the condition to determine which command to run
+   */
+  @SuppressWarnings("this-escape")
+  public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
+    m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
+    m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
+
+    //CommandScheduler.getInstance().registerComposedCommands(whileTrue);
+
+    // addRequirements(whileTrue.getRequirements());
+  }
+
+  @Override
+  public void initialize() {
+    if(m_condition.getAsBoolean())
+      m_whileTrue.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_whileTrue.execute();
+
+    System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " +  m_condition.getAsBoolean());
+
+    if(!m_whileTrue.isFinished())
+      return;
+
+    if(m_condition.getAsBoolean()){
+      m_whileTrue.end(false);
+      m_whileTrue.initialize();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_whileTrue.end(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_whileTrue.runsWhenDisabled();
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+      return InterruptionBehavior.kCancelSelf;
+    } else {
+      return InterruptionBehavior.kCancelIncoming;
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
+    builder.addStringProperty(
+        "selected",
+        () -> {
+          if (m_whileTrue == null) {
+            return "null";
+          } else {
+            return m_whileTrue.getName();
+          }
+        },
+        null);
+  }
+}

src/main/java/frc4388/robot/commands/waitSupplier.java

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitSupplier extends Command {
+  /** Creates a new waitSupplier. */
+  private final Supplier<Boolean> truth;
+  public waitSupplier(Supplier<Boolean> truth) {
+    this.truth = truth;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return truth.get();
+  }
+}

src/main/java/frc4388/robot/subsystems/Climber.java

@@ -1,43 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.subsystems;
-
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.ClimberConstants;
-
-public class Climber extends SubsystemBase {
-  /** Creates a new Climber. */
-
-  public TalonFX climberMotor;
-
-  public Climber(TalonFX climberTalonFX) {
-    climberMotor = climberTalonFX;
-    climberMotor.setNeutralMode(NeutralModeValue.Brake);
-
-  }
-
-  public void stopClimber(){
-    climberMotor.set(0);
-  }
-
-
-  public void climberOut(){
-    climberMotor.set(ClimberConstants.CLIMBER_SPEED);
-
-  }
-
-  public void climberIn(){
-    climberMotor.set(-ClimberConstants.CLIMBER_SPEED);
-
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
-}

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -16,6 +16,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -47,8 +48,12 @@ public class Elevator extends Subsystem {
 
   private boolean disableAutoIntake = false;
 
+  private boolean seededZeroEndefector = false;
+  private boolean seededZeroElevator = false;
+
   private DigitalInput basinBeamBreak;
   private DigitalInput endeffectorLimitSwitch;
+  private DigitalInput intakeIR;
 
   public enum CoordinationState {
     Waiting, // for coral into the though
@@ -56,6 +61,7 @@ public class Elevator extends Subsystem {
     Ready, // Has coral in endefector
     Hovering, // Has coral in endefector
     L2Score,
+    L2ScoreLeave,
     PrimedThree, // Arm and elevator Waiting to score in the level 3 position
     ScoringThree, // Arm and elevator in the level three position
     PrimedFour, // Arm and elevator Waiting to score in the level 4 position
@@ -68,13 +74,15 @@ public class Elevator extends Subsystem {
 
   private CoordinationState currentState;
 
-  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
+  // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
+  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
     elevatorMotor = elevatorTalonFX;
     endeffectorMotor = endeffectorTalonFX;
     this.led = led;
 
     this.basinBeamBreak = basinLimitSwitch;
     this.endeffectorLimitSwitch = endeffectorLimitSwitch;
+    this.intakeIR = intakeDigitalInput;
 
     elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
     endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
@@ -104,12 +112,15 @@ public class Elevator extends Subsystem {
 
 
   public void transitionState(CoordinationState state) {
+    // elevatorMotor.enable();
+    
+
     currentState = state;
     switch (currentState) {
       case Waiting: {
         wait = System.currentTimeMillis() + maxWait;
         PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
         led.setMode(LEDConstants.WAITING_PATTERN);
         break;
       }
@@ -122,78 +133,85 @@ public class Elevator extends Subsystem {
       }
 
       case Ready: {
-        PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         led.setMode(LEDConstants.DOWN_PATTERN);
         break;
       }
 
       case Hovering: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR);
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
         led.setMode(LEDConstants.READY_PATTERN);
         break;
       }
 
       case L2Score: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case L2ScoreLeave: {
+        PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
       
       case PrimedFour: {
         PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
-        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
+        led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case ScoringFour: {
         PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
+        led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case PrimedThree: {
         PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
-        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
+        led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
       
       case ScoringThree: {
         PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
+        led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case BallRemoverL2Primed: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
-        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
+        led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case BallRemoverL2Go: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
+        led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
       case BallRemoverL3Primed: {
         PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
         break;
       }
 
       case BallRemoverL3Go: {
         PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
         PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN);
+        led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
 
@@ -237,11 +255,15 @@ public class Elevator extends Subsystem {
   //   }
   // }
 
+  public boolean getEndeffectorLimit() {
+    return endeffectorLimitSwitch.get();
+  }
+
   private void periodicWaiting() {
     if (!basinBeamBreak.get()) 
       transitionState(CoordinationState.Ready);
-    if(!endeffectorLimitSwitch.get())
+    // if(!endeffectorLimitSwitch.get())
-      transitionState(CoordinationState.Hovering);
+    //   transitionState(CoordinationState.Hovering);
   }
 
   // private void periodicWaitingTripped() {
@@ -253,7 +275,7 @@ public class Elevator extends Subsystem {
     if (elevatorAtReference() && !endeffectorLimitSwitch.get())
       transitionState(CoordinationState.Hovering);
     if(elevatorAtReference() && endeffectorLimitSwitch.get())
-      transitionState(CoordinationState.Waiting);
+      transitionState(CoordinationState.Hovering);
   }
 
   private void periodicScoring() {
@@ -288,11 +310,34 @@ public class Elevator extends Subsystem {
   @Override
   public void periodic() {
 
+    // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble();
+    // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble();
+    // double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
+    // double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
+
+
+    // if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
+    //   PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
+    // }
+
     // This method will be called once per scheduler run
+    // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
+    // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
     SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
     SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
+    SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
     SmartDashboard.putString("State", currentState.toString());
 
+    if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) {
+      endeffectorMotor.setPosition(0);
+      seededZeroEndefector = !seededZeroEndefector;
+    }
+
+    if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) {
+      elevatorMotor.setPosition(0);
+      seededZeroElevator = !seededZeroElevator;
+    }
+    
     if (disableAutoIntake) return;
 
     if (currentState == CoordinationState.Waiting) {
@@ -302,21 +347,32 @@ public class Elevator extends Subsystem {
     } else if (currentState == CoordinationState.Ready) {
       periodicReady();
     }
+
+    if(!intakeIR.get()){
+      led.setMode(LEDConstants.DOWN_PATTERN);
+    }
+
+    
     // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
     //   periodicScoring();
     // }
   }
 
-  public boolean isL4Primed(){
+  public boolean isL4Primed() {
     return currentState == CoordinationState.PrimedFour;
   }
 
-  public boolean isL3Primed(){
+  public boolean isL3Primed() {
     return currentState == CoordinationState.PrimedThree;
   }
 
-  public boolean hasCoral(){
+  public boolean hasCoral() {
-    return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get();
+    return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
+  }
+
+  public boolean readyToMove() {
+    return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get();
+    // return hasCoral();
   }
 
   public void armShuffle(){

src/main/java/frc4388/robot/subsystems/Lidar.java

@@ -15,13 +15,31 @@ import frc4388.utility.Status.ReportLevel;
 // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
 public class Lidar extends Subsystem {
 
-    private double distance = -1;
+    private Counter LidarPWM;
-    Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL);
+    private String name = "Lidar";
 
-    public Lidar() {
+    private double distance = -1;
+    public Lidar(int port, String name) {
+        this.name = name;
+        LidarPWM = new Counter(port);
         LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
         LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
         LidarPWM.reset();
+
+        
+    subsystemLayout = Shuffleboard.getTab("Subsystems")
+    .getLayout(getSubsystemName(), BuiltInLayouts.kList)
+    .withSize(2, 2);
+
+    sbDistance = subsystemLayout
+    .add("Distance", 0)
+    .withWidget(BuiltInWidgets.kGraph)
+    .getEntry();
+
+    sbWithinDistance = subsystemLayout
+    .   add("Within Distance", 0)
+    .withWidget(BuiltInWidgets.kBooleanBox)
+    .getEntry();
     }
 
     @Override
@@ -41,23 +59,13 @@ public class Lidar extends Subsystem {
         return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
     }
 
-    ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
+    ShuffleboardLayout subsystemLayout;
-            .getLayout(getSubsystemName(), BuiltInLayouts.kList)
+    GenericEntry sbDistance;
-            .withSize(2, 2);
+    GenericEntry sbWithinDistance;
-
-    GenericEntry sbDistance = subsystemLayout
-            .add("Distance", 0)
-            .withWidget(BuiltInWidgets.kGraph)
-            .getEntry();
-
-    GenericEntry sbWithinDistance = subsystemLayout
-            .add("Within Distance", 0)
-            .withWidget(BuiltInWidgets.kBooleanBox)
-            .getEntry();
 
     @Override
     public String getSubsystemName() {
-        return "Lidar";
+        return "Lidar " + name;
     }
 
     @Override

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -58,6 +58,8 @@ public class SwerveDrive extends Subsystem {
     public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
                                                                                            // 25%
 
+    public double lastOdomSpeed;
+
     public Pose2d initalPose2d = null;
 
 
@@ -116,6 +118,7 @@ public class SwerveDrive extends Subsystem {
     }
 
     public void setOdoPose(Pose2d pose) {
+        if (pose == null) return;
         initalPose2d = pose;
         swerveDriveTrain.resetPose(pose);
     }
@@ -141,7 +144,7 @@ public class SwerveDrive extends Subsystem {
         if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
             return; // don't bother doing swerve drive math and return early.
 
-            leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
         
         stopped = false;
         if (fieldRelative) {
@@ -184,10 +187,12 @@ public class SwerveDrive extends Subsystem {
     public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
         stopped = false;
         // Create robot-relative speeds.
+        if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
         swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
             .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
             .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
             .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+        
     }
 
 
@@ -211,18 +216,33 @@ public class SwerveDrive extends Subsystem {
                 .withTargetDirection(rightStick.getAngle()));
     }
 
+    public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+        leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withTargetDirection(heading);
+        ctrl.HeadingController.setPID(
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+        );
+        swerveDriveTrain.setControl(ctrl);
+    }
+
     public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
         leftStick = leftStick.rotateBy(heading);
 
         var ctrl = new SwerveRequest.FieldCentricFacingAngle()
             .withVelocityX(leftStick.getX() * speedAdjust)
             .withVelocityY(leftStick.getY() * speedAdjust)
-            .withTargetDirection(Rotation2d.fromDegrees(rotTarget));
+            .withTargetDirection(heading);
-        ctrl.HeadingController.setPID(
+        // ctrl.HeadingController.setPID(
-            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+        //     SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
-            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+        //     SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
-            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+        //     SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
-        );
+        // );
         swerveDriveTrain.setControl(ctrl);
     }
 
@@ -288,6 +308,7 @@ public class SwerveDrive extends Subsystem {
         swerveDriveTrain.tareEverything();
         robotKnowsWhereItIs = false;
         rotTarget = 0;
+        // vision.resetRotations();
     }
 
 
@@ -313,9 +334,13 @@ public class SwerveDrive extends Subsystem {
         SmartDashboard.putNumber("RotTartget", rotTarget);
 
         double time = Vision.getTime();
+        double freq =  swerveDriveTrain.getOdometryFrequency();
 
+        Optional<Pose2d> curpose = swerveDriveTrain.samplePoseAt(time);
+        Optional<Pose2d> lastPose = swerveDriveTrain.samplePoseAt(time - freq);
         
-        vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
+        vision.setLastOdomPose(curpose);
+        setLastOdomSpeed(curpose, lastPose, freq);
 
         if (vision.isTag()) {
             Pose2d pose = vision.getPose2d();
@@ -326,7 +351,9 @@ public class SwerveDrive extends Subsystem {
                 rotTarget += deltaAngle;
             }
             
-            swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
+            vision.addVisionMeasurement(swerveDriveTrain);
+            // swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
+            //back to the ~~future~~ *past*
         }
 
         // if(e.isPresent())
@@ -392,11 +419,35 @@ public class SwerveDrive extends Subsystem {
         setToSlow();
     }
 
+    public void startTurboPeriod() {
+        tmp_gear_index = gear_index;
+        setToTurbo();
+    }
+
     public void endSlowPeriod() {
         setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
         gear_index = tmp_gear_index;
     }
 
+
+
+    public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
+        if(curPose.isPresent() && lastPose.isPresent()){
+            this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
+        
+
+            // double rotDiff = curPose.get().getRotation().getDegrees() - lastPose.get().getRotation().getDegrees();
+    
+            // if(rotDiff >= 180){
+            //     vision.subtractRotation();
+            // }else if(rotDiff <= -180){
+            //     vision.addRotation();
+            // }
+            SmartDashboard.putNumber("Speed", lastOdomSpeed);
+        }
+    }
+
+
     @Override
     public String getSubsystemName() {
         return "Swerve Drive Controller";

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -9,6 +9,8 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 
+import java.time.Instant;
+import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
 
@@ -24,6 +26,9 @@ import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
 
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
@@ -45,17 +50,24 @@ public class Vision extends Subsystem {
 
     private PhotonCamera[] cameras;
     private PhotonPoseEstimator[] estimators;
+    private List<EstimatedRobotPose> poses = new ArrayList<>();
 
     private boolean isTagDetected = false;
     private boolean isTagProcessed = false;
-    private Pose2d lastVisionPose = new Pose2d();
+
+    private double lastLatency = 0;
+
+    public double getLastLatency() {
+        return lastLatency;
+    }
+
+    public Pose2d lastVisionPose = new Pose2d();
     private Pose2d lastPhysOdomPose = new Pose2d();
 
     private Matrix<N3, N1> curStdDevs;
 
     private Field2d field = new Field2d();
     
-    
     ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
     .getLayout(getSubsystemName(), BuiltInLayouts.kList)
     .withSize(2, 2);
@@ -92,25 +104,42 @@ public class Vision extends Subsystem {
         photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
 
         this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight};
+        // resetRotations();
     }
 
     @Override
     public void periodic() {
         update();
         field.setRobotPose(getPose2d());
+        // cameras[0].
     }
 
+    // public int[] rotations;
+    // public Instant[] lastUpdateTimes;
+
+    // public void resetRotations(){
+    //     rotations = new int[cameras.length];
+    //     lastUpdateTimes = new Instant[cameras.length];
+    // }
+
+    private Instant lastVisionTime = null;
+
 
     private void update() {
         isTagProcessed = false;
         isTagDetected = false;
 
+        Instant now = Instant.now();
 
         int cams = 0;
 
-        double X = 0;
+        // double X = 0;
-        double Y = 0;
+        // double Y = 0;
-        double Yaw = 0;
+        // double Yaw = 0;
+        double latency = 0;
+
+        // Pose2d pose = null;
+        poses.clear();
 
         for(int i = 0; i < cameras.length; i++){
             PhotonCamera camera = cameras[i];
@@ -124,6 +153,7 @@ public class Vision extends Subsystem {
 
             
             var result = results.get(results.size()-1);
+            latency += result.getTimestampSeconds();
 
             isTagDetected = isTagDetected | result.hasTargets();
 
@@ -137,19 +167,39 @@ public class Vision extends Subsystem {
             if(estimatedRobotPose.isEmpty())
                 continue;
             
-            Pose2d pose = estimatedRobotPose.get().estimatedPose.toPose2d();
+            poses.add(estimatedRobotPose.get());
-            X += pose.getX();
+
-            Y += pose.getY();
+            // if(pose == null)
-            Yaw += pose.getRotation().getDegrees();
+            //     pose = estimatedRobotPose.get().estimatedPose.toPose2d();
-            cams++;
+            // else
+            //     pose = pose.interpolate(pose, 0.5);
+            // X += pose.getX();
+            // Y += pose.getY();
+
+            // if(X > 6)
+
+            // Yaw += (pose.getRotation().getDegrees() + 180) % 360;
+            // cams++;
+
             isTagProcessed = true;
         
             
         }
 
-        if(isTagProcessed){
+        // lastLatency = latency / cams;
-            lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams));
+
-        }
+        // if(isTagProcessed){
+
+
+        //     lastVisionPose = pose;
+        //     // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
+        //     // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360));
+
+        //     // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations());
+        //     // SmartDashboard.putNumber("Rotations", rotations);
+
+        //     lastVisionTime = now;
+        // }
     }
 
 
@@ -179,66 +229,66 @@ public class Vision extends Subsystem {
             return visionEst; // Will be empty
 
         visionEst = estimator.update(change);
-        updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
+        // updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
 
         return visionEst;
     }
 
 
-    /**
+    // /**
-     * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
+    //  * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
-     * deviations based on number of tags, estimation strategy, and distance from the tags.
+    //  * deviations based on number of tags, estimation strategy, and distance from the tags.
-     *
+    //  *
-     * @param estimatedPose The estimated pose to guess standard deviations for.
+    //  * @param estimatedPose The estimated pose to guess standard deviations for.
-     * @param targets All targets in this camera frame
+    //  * @param targets All targets in this camera frame
-     */
+    //  */
-    private void updateEstimationStdDevs(
+    // private void updateEstimationStdDevs(
-            Optional<EstimatedRobotPose> estimatedPose, 
+    //         Optional<EstimatedRobotPose> estimatedPose, 
-            List<PhotonTrackedTarget> targets,
+    //         List<PhotonTrackedTarget> targets,
-            PhotonPoseEstimator estimator) {
+    //         PhotonPoseEstimator estimator) {
-        if (estimatedPose.isEmpty()) {
+    //     if (estimatedPose.isEmpty()) {
-            // No pose input. Default to single-tag std devs
+    //         // No pose input. Default to single-tag std devs
-            curStdDevs = VisionConstants.kSingleTagStdDevs;
+    //         curStdDevs = VisionConstants.kSingleTagStdDevs;
 
-        } else {
+    //     } else {
-            // Pose present. Start running Heuristic
+    //         // Pose present. Start running Heuristic
-            var estStdDevs = VisionConstants.kSingleTagStdDevs;
+    //         var estStdDevs = VisionConstants.kSingleTagStdDevs;
-            int numTags = 0;
+    //         int numTags = 0;
-            double avgDist = 0;
+    //         double avgDist = 0;
 
-            // Precalculation - see how many tags we found, and calculate an average-distance metric
+    //         // Precalculation - see how many tags we found, and calculate an average-distance metric
-            for (var tgt : targets) {
+    //         for (var tgt : targets) {
-                var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
+    //             var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
-                if (tagPose.isEmpty()) continue;
+    //             if (tagPose.isEmpty()) continue;
                 
-                double distance = tagPose
+    //             double distance = tagPose
-                .get()
+    //             .get()
-                .toPose2d()
+    //             .toPose2d()
-                .getTranslation()
+    //             .getTranslation()
-                .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+    //             .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
                 
-                if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
+    //             if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
-                    numTags++;
+    //                 numTags++;
-                    avgDist += distance;
+    //                 avgDist += distance;
-                }
+    //             }
-            }
+    //         }
 
-            if (numTags == 0) {
+    //         if (numTags == 0) {
-                // No tags visible. Default to single-tag std devs
+    //             // No tags visible. Default to single-tag std devs
-                curStdDevs = VisionConstants.kSingleTagStdDevs;
+    //             curStdDevs = VisionConstants.kSingleTagStdDevs;
-            } else {
+    //         } else {
-                // One or more tags visible, run the full heuristic.
+    //             // One or more tags visible, run the full heuristic.
-                avgDist /= numTags;
+    //             avgDist /= numTags;
-                // Decrease std devs if multiple targets are visible
+    //             // Decrease std devs if multiple targets are visible
-                if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
+    //             if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
-                // Increase std devs based on (average) distance
+    //             // Increase std devs based on (average) distance
-                if (numTags == 1 && avgDist > 4)
+    //             if (numTags == 1 && avgDist > 4)
-                    estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    //                 estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
-                else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+    //             else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
-                curStdDevs = estStdDevs;
+    //             curStdDevs = estStdDevs;
-            }
+    //         }
-        }
+    //     }
-    }
+    // }
 
     /**
      * Returns the latest standard deviations of the estimated pose from {@link
@@ -253,21 +303,23 @@ public class Vision extends Subsystem {
 
 
 
-
-
-
-
-
     public void setLastOdomPose(Optional<Pose2d> pose){
         if(pose.isPresent())
             lastPhysOdomPose = pose.get();
     }
 
+    // public double getLastOdomSpeed(){
+    //     return lastOdomSpeed;
+    // }
+
     public Pose2d getPose2d() {
-        if(isTagDetected && isTagProcessed)
+        if(lastPhysOdomPose != null)
-            return lastVisionPose;
-        else
             return lastPhysOdomPose;
+        return new Pose2d();
+        // if(isTagDetected && isTagProcessed)
+        //     // return lastVisionPose;
+        // else
+        //     return lastPhysOdomPose;
     }
 
     public static double getTime() {
@@ -279,7 +331,11 @@ public class Vision extends Subsystem {
     }
 
 
-
+    public void addVisionMeasurement( SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
+        for(EstimatedRobotPose pose : poses){
+            drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds));
+        }
+    }
 
 
 

src/main/java/frc4388/utility/ReefPositionHelper.java

@@ -5,6 +5,7 @@ import java.util.Optional;
 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.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc4388.robot.Constants.AutoConstants;
@@ -14,7 +15,8 @@ public class ReefPositionHelper {
     public enum Side {
         LEFT,
         RIGHT,
-        CENTER
+        CENTER,
+        FAR_LEFT
     }
 
     public static final Pose2d[] RED_TAGS = {
@@ -57,6 +59,8 @@ public class ReefPositionHelper {
             }
         }
         
+        System.out.println(minPos.getRotation().getDegrees());
+
         return minPos;
     }
 
@@ -81,11 +85,14 @@ public class ReefPositionHelper {
         switch(side) {
             case LEFT:
                 return -(AutoConstants.X_SCORING_POSITION_OFFSET);
+            case FAR_LEFT:
+                return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8));
             case RIGHT:
                 return (AutoConstants.X_SCORING_POSITION_OFFSET);
             case CENTER:
                 return 0;
         }
+        assert false;
         return 0;
     }
 

src/main/java/frc4388/utility/Trim.java

@@ -81,21 +81,22 @@ public class Trim {
     }
 
     public boolean load() {
-        try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
+        // try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
-            double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
+        //     double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
-            currentValue = fileValue;
+        //     currentValue = fileValue;
-            clampModify();
+        //     clampModify();
-            modified = false;
+        //     modified = false;
-            if (fileValue != currentValue) {
+        //     if (fileValue != currentValue) {
-                System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
+        //         System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
-                modified = true;
+        //         modified = true;
-            }
+        //     }
-            return true;
+        //     return true;
-        } catch (Exception e) {
+        // } catch (Exception e) {
-            // e.printStackTrace();
+        //     // e.printStackTrace();
-            System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
+        //     System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
-            return false;
+        //     return false;
-        }
+        // }
+        return false;
     }
 
     public void dump() {