fixing dt bindings divide by zero flaw

.OutlineViewer/outlineviewer.json

@@ -8,9 +8,18 @@
   "NetworkTables Settings": {
     "mode": "Client (NT4)"
   },
+  "client@1": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
   "client@2": {
     "open": true
   },
+  "client@3": {
+    "open": true
+  },
   "client@4": {
     "Publishers": {
       "open": true

src/main/deploy/pathplanner/autos/1 L4 Center.auto

@@ -4,6 +4,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift L4"
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -21,6 +27,12 @@
           "data": {
             "pathName": "H Backup"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "HP Pickup"
+          }
         }
       ]
     }

src/main/deploy/pathplanner/autos/1L4 + Algae Center.auto

@@ -0,0 +1,74 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to H"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "H Backup"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Pickup Algae L2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HG Algae"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HG to Barge"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Algae"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Post-Barge Backup"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/2.5 Coral Left.auto

@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to 30 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "30 Right to HP"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Collect Coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP to 330 Right"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Backup"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/3 Coral Left.auto

@@ -10,18 +10,6 @@
             "pathName": "Start to 30 Right"
           }
         },
-        {
-          "type": "named",
-          "data": {
-            "name": "Lift L4"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "J Approach"
-          }
-        },
         {
           "type": "named",
           "data": {
@@ -56,19 +44,44 @@
         {
           "type": "path",
           "data": {
-            "pathName": "HP to 330 Left"
+            "pathName": "HP to 330 Right"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "Lift L4"
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Backup"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Collect Coral"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "K Approach"
+            "pathName": "HP to K"
           }
         },
         {

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

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "fein"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/paths/30 Right to HP.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.649657534252619,
+        "x": 4.843032786885245,
-        "y": 6.474186643842743
+        "y": 6.30266393442623
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.174795081967213,
+        "x": 1.1268442622950818,
-        "y": 7.321618852459016
+        "y": 7.201741803278688
       },
       "prevControl": {
-        "x": 2.031259123063103,
+        "x": 2.287270519874242,
-        "y": 6.375000701774084
+        "y": 6.774371912194027
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 5.0,
     "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
@@ -45,10 +45,10 @@
     "rotation": -53.97262661489646
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -120.06858282186238
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/330 Right to HP.path

@@ -45,8 +45,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 5.0,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -57,10 +57,10 @@
     "rotation": -53.98486432191523
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -59.99999999999999
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/E to HP.path

@@ -0,0 +1,73 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.958476027397261,
+        "y": 2.837970890410959
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.619606164383562,
+        "y": 1.455607876712329
+      },
+      "isLocked": false,
+      "linkedName": "E"
+    },
+    {
+      "anchor": {
+        "x": 1.1569777397260272,
+        "y": 1.0198630136986298
+      },
+      "prevControl": {
+        "x": 2.7647260273972605,
+        "y": 1.485659246575342
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Right HP"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "HP Pickup",
+      "waypointRelativePos": 0.22857142857142831,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "HP Pickup"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 54.162347045721745
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 120.25643716352937
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/H Backup.path

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.7,
+        "x": 5.758260140458621,
-        "y": 4.3
+        "y": 4.193633481772718
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.053790983606557,
+        "x": 6.112051124065178,
-        "y": 4.312704918032787
+        "y": 4.206338399805505
       },
       "isLocked": false,
       "linkedName": "H"
     },
     {
       "anchor": {
-        "x": 6.389446721311475,
+        "x": 6.5,
-        "y": 4.3
+        "y": 4.021985060730393
       },
       "prevControl": {
-        "x": 6.1394487099079695,
+        "x": 6.250001988596495,
-        "y": 4.300997143065429
+        "y": 4.022982203795822
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Behind HG Face"
     }
   ],
   "rotationTargets": [],
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 3.5,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 0.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -45,10 +45,10 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Center",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/HG Algae.path

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 5.289041095890411,
+        "x": 6.5,
-        "y": 2.9732020547945206
+        "y": 4.021985060730393
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.950171232876712,
+        "x": 6.224227344787433,
-        "y": 1.6208904109589037
+        "y": 4.009974315068493
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Behind HG Face"
     },
     {
       "anchor": {
-        "x": 1.4424657534246574,
+        "x": 5.872671261177789,
-        "y": 0.7944777397260271
+        "y": 4.021985060730393
       },
       "prevControl": {
-        "x": 2.389083904109589,
+        "x": 6.125859753477119,
-        "y": 2.5374571917808213
+        "y": 4.004837296105254
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "HG Algae"
     }
   ],
   "rotationTargets": [],
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 3.5,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 54.162347045721745
+    "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Center",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 120.96375653207336
+    "rotation": 180.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/HG to Barge.path

@@ -0,0 +1,71 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.872671261177789,
+        "y": 4.021985060730393
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.829105562465259,
+        "y": 3.8676166530198106
+      },
+      "isLocked": false,
+      "linkedName": "HG Algae"
+    },
+    {
+      "anchor": {
+        "x": 7.032020547945206,
+        "y": 4.761258561643835
+      },
+      "prevControl": {
+        "x": 7.262336691900294,
+        "y": 4.359747737103425
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Pre-Barge"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.17234468937875755,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Processor Position",
+      "waypointRelativePos": 0.188095238095238,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Processor Position"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 300.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/HP to 330 Right.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.0019467213114757,
+        "y": 6.434528688524591
+      },
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    },
+    {
+      "anchor": {
+        "x": 3.609900518622585,
+        "y": 5.005924534374863
+      },
+      "prevControl": {
+        "x": 3.2943611350609414,
+        "y": 5.531823506977602
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "L"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.25,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/HP to D.path

@@ -0,0 +1,73 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.187029109589041,
+        "y": 0.9747859589041092
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.268878424657535,
+        "y": 1.0949914383561645
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.011857876712329,
+        "y": 2.837970890410959
+      },
+      "prevControl": {
+        "x": 3.3657534246575342,
+        "y": 1.9364297945205475
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.27142857142857096,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Lift L4"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 58.57043438516136
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 54.24611274556325
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HP to K.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9266540815180775,
+        "y": 6.744320542331013
+      },
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    },
+    {
+      "anchor": {
+        "x": 3.9679303278688525,
+        "y": 5.211782786885246
+      },
+      "prevControl": {
+        "x": 2.975085616438356,
+        "y": 6.023416095890411
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -60.49491285058726
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/J Approach.path

@@ -17,11 +17,11 @@
     {
       "anchor": {
         "x": 4.974897540983607,
-        "y": 5.115881147540984
+        "y": 5.235758196721312
       },
       "prevControl": {
         "x": 5.058811475409836,
-        "y": 5.403586065573771
+        "y": 5.523463114754099
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 3.5,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/J Backup.path

@@ -4,12 +4,12 @@
     {
       "anchor": {
         "x": 4.974897540983607,
-        "y": 5.115881147540984
+        "y": 5.235758196721312
       },
       "prevControl": null,
       "nextControl": {
         "x": 5.11855393929902,
-        "y": 5.383513333352196
+        "y": 5.503390382532524
       },
       "isLocked": false,
       "linkedName": "J"
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 3.5,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/K Approach.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.6202868852459016,
+        "x": 3.35655737704918,
-        "y": 5.859118852459017
+        "y": 5.78719262295082
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.7970635805425386,
+        "x": 3.402629510813402,
-        "y": 5.68234215716238
+        "y": 5.541474576217554
       },
       "isLocked": false,
       "linkedName": "Before K"
     },
     {
       "anchor": {
-        "x": 3.8720286885245896,
+        "x": 3.6322745901639335,
-        "y": 5.043954918032787
+        "y": 5.019979508196721
       },
       "prevControl": {
-        "x": 3.836065573770491,
+        "x": 3.596311475409835,
-        "y": 5.295696721311475
+        "y": 5.271721311475409
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 1.0,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 0.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": -59.69923999693802
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/L Backup.path

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.174795081967213,
+        "x": 3.609900518622585,
-        "y": 7.321618852459016
+        "y": 5.005924534374863
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.4670039860768025,
+        "x": 3.47740078362179,
-        "y": 7.096233578486413
+        "y": 5.217924110376135
       },
       "isLocked": false,
-      "linkedName": "HP Left Position"
+      "linkedName": "L"
     },
     {
       "anchor": {
-        "x": 3.6202868852459016,
+        "x": 1.1268442622950818,
-        "y": 5.859118852459017
+        "y": 7.201741803278688
       },
       "prevControl": {
-        "x": 3.304747501684258,
+        "x": 1.3306352459016395,
-        "y": 6.385017825061756
+        "y": 6.926024590163935
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "Before K"
+      "linkedName": "HP Left Position"
     }
   ],
   "rotationTargets": [],
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 5.0,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
-    "rotation": -59.69923999693802
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
     "velocity": 0,
     "rotation": -53.97262661489646
   },
-  "useDefaultConstraints": true
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -59.69923999693802
+  },
+  "useDefaultConstraints": false
 }

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

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 12.272,
+        "x": 3.756421232876712,
-        "y": 2.975
+        "y": 5.227054794520548
       },
       "prevControl": {
-        "x": 10.940715672291898,
+        "x": 2.756421232876712,
-        "y": 2.975
+        "y": 5.227054794520548
       },
       "nextControl": null,
       "isLocked": false,
@@ -30,22 +30,11 @@
   ],
   "rotationTargets": [],
   "constraintZones": [],
-  "pointTowardsZones": [
+  "pointTowardsZones": [],
-    {
-      "fieldPosition": {
-        "x": 0.4,
-        "y": 5.5
-      },
-      "rotationOffset": 0.0,
-      "minWaypointRelativePos": 0.15,
-      "maxWaypointRelativePos": 0.4,
-      "name": "Point Towards Zone"
-    }
-  ],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 3.5,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -53,10 +42,10 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 59.99999999999999
+    "rotation": -59.18537788806707
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 0.0

src/main/deploy/pathplanner/paths/Post-Barge Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.032020547945206,
+        "y": 4.761258561643835
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.71311475409836,
+        "y": 4.9480532786885245
+      },
+      "isLocked": false,
+      "linkedName": "Pre-Barge"
+    },
+    {
+      "anchor": {
+        "x": 5.933913934426228,
+        "y": 5.247745901639345
+      },
+      "prevControl": {
+        "x": 6.641188524590164,
+        "y": 5.043954918032786
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Right Start to E.path

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.602996575342465,
+        "x": 7.572945205479453,
-        "y": 2.0115582191780823
+        "y": 0.4939640410958907
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.491095890410959,
+        "x": 6.461044520547946,
-        "y": 1.9965325342465756
+        "y": 0.4789383561643841
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.289041095890411,
+        "x": 4.958476027397261,
-        "y": 2.9882277397260277
+        "y": 2.837970890410959
       },
       "prevControl": {
-        "x": 6.130479452054795,
+        "x": 5.799914383561644,
-        "y": 2.11673801369863
+        "y": 1.9664811643835614
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "E"
     }
   ],
   "rotationTargets": [],
@@ -34,14 +34,26 @@
   "eventMarkers": [
     {
       "name": "Lift L4",
-      "waypointRelativePos": 0.7142857142857124,
+      "waypointRelativePos": 0.4261904761904757,
       "endWaypointRelativePos": null,
-      "command": null
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Lift L4"
+              }
+            }
+          ]
+        }
+      }
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 3.5,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/Start to 30 Right.path

@@ -3,38 +3,50 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.588217213114754,
+        "x": 7.150967037968244,
-        "y": 7.537397540983607
+        "y": 7.5521014571037055
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.3534836065573765,
+        "x": 5.916233431410867,
-        "y": 6.51844262295082
+        "y": 6.533146539070919
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.286577868852459,
+        "x": 4.974897540983607,
-        "y": 5.955020491803278
+        "y": 5.235758196721312
       },
       "prevControl": {
-        "x": 5.872579581181226,
+        "x": 5.560899253312374,
-        "y": 6.826510217830675
+        "y": 6.107247922748709
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "Before J"
+      "linkedName": "J"
     }
   ],
   "rotationTargets": [],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 3.5,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 1.25,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -45,10 +57,10 @@
     "rotation": -119.71497744813712
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
   "idealStartingState": {
     "velocity": 0,
     "rotation": -90.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/Start to H.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 5.7,
+        "x": 5.758260140458621,
-        "y": 4.3
+        "y": 4.193633481772718
       },
       "prevControl": {
-        "x": 6.347336065573771,
+        "x": 6.405596206032391,
-        "y": 4.2640368852459005
+        "y": 4.157670367018619
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,14 +34,19 @@
   "eventMarkers": [
     {
       "name": "Lift L4",
-      "waypointRelativePos": 0.08214624881291864,
+      "waypointRelativePos": 0.10238095238095252,
       "endWaypointRelativePos": null,
-      "command": null
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4.0,
+    "maxVelocity": 3.5,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -52,10 +57,10 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Center",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -1,70 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 7.600204918039607,
-        "y": 6.374590163938041
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 7.600204918032786,
-        "y": 7.573360655737705
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 6.209631147540984,
-        "y": 6.074897540983606
-      },
-      "prevControl": {
-        "x": 7.120696721311476,
-        "y": 6.074897540983606
-      },
-      "nextControl": {
-        "x": 4.745363537952068,
-        "y": 6.074897540983606
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 4.8670081967213115,
-        "y": 6.973975409836065
-      },
-      "prevControl": {
-        "x": 5.826024590163935,
-        "y": 6.9979508196721305
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 2.5,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 400.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 90.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 180.0
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/settings.json

@@ -2,10 +2,13 @@
   "robotWidth": 0.8763,
   "robotLength": 0.8763,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "Left Paths",
+    "Center"
+  ],
   "autoFolders": [],
-  "defaultMaxVel": 4.0,
+  "defaultMaxVel": 3.5,
-  "defaultMaxAccel": 2.5,
+  "defaultMaxAccel": 1.75,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 400.0,
   "defaultNominalVoltage": 12.0,

src/main/java/frc/robot/Robot.java

@@ -35,7 +35,7 @@ public Robot() {
 
     if (isReal()) {
         Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
-        Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+        //Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
         new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
     } else {
         setUseTiming(false); // Run as fast as possible

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

@@ -7,11 +7,9 @@ package frc.robot;
 import frc.robot.constants.ManipulatorPivotConstants;
 import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
-import frc.robot.constants.ManipulatorConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.ManipulatorPivot;
-import frc.robot.subsystems.Vision;
 import frc.robot.subsystems.ClimberPivot;
 import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
@@ -22,9 +20,9 @@ import java.util.function.IntSupplier;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.events.EventTrigger;
+import com.pathplanner.lib.commands.PathPlannerAuto;
-import com.pathplanner.lib.path.EventMarker;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -44,6 +42,8 @@ public class RobotContainer {
 	private Elevator elevator;
 	//private ElevatorSysID elevator;
 
+	//private Indexer indexer;
+
 	private Manipulator manipulator;
 
 	private ManipulatorPivot manipulatorPivot;
@@ -65,6 +65,8 @@ public class RobotContainer {
 		elevator = new Elevator();
 		//elevator = new ElevatorSysID();
 
+		//indexer = new Indexer();
+
 		manipulator = new Manipulator();
 
 		manipulatorPivot = new ManipulatorPivot();
@@ -75,6 +77,9 @@ public class RobotContainer {
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
 		autoChooser = AutoBuilder.buildAutoChooser();
+		autoChooser.addOption("One Coral Left", new PathPlannerAuto("One Coral Left", true));
+		autoChooser.addOption("2.5 Coral Right", new PathPlannerAuto("2.5 Coral Left", true));
+		autoChooser.addOption("3 Coral Right", new PathPlannerAuto("3 Coral Left", true));
 
 		closestTag = drivetrain::getClosestTag;
 
@@ -106,9 +111,9 @@ public class RobotContainer {
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
+				() -> driver.getLeftY(),
-				() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
+				() -> driver.getLeftX(),
-				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
+				() -> driver.getRightX(), 
 				() -> true
 			)
 		);
@@ -116,6 +121,10 @@ public class RobotContainer {
 		elevator.setDefaultCommand(
 			elevator.maintainPosition()
 		);
+
+		//indexer.setDefaultCommand(
+		//	indexer.runIndexer(() -> 0)
+		//);
 		
 		
 		manipulatorPivot.setDefaultCommand(
@@ -124,9 +133,7 @@ public class RobotContainer {
 		
 
 		manipulator.setDefaultCommand(
-			manipulator.runUntilCollected(
+			manipulator.runManipulator(() -> 0.0, false)
-				() -> 0.0
-			)
 		);
 
 		//Driver inputs
@@ -141,6 +148,9 @@ public class RobotContainer {
 
 		driver.leftTrigger().whileTrue( 
 			manipulator.runUntilCollected(() -> 0.75)
+			//.alongWith(indexer.runIndexer(() -> .75))
+			.until(() -> manipulator.getCoralBeamBreak() == false)
+			.andThen(manipulator.retractCommand(() -> .1))
 		);
 
 		driver.start().and(driver.back()).onTrue(
@@ -152,23 +162,36 @@ public class RobotContainer {
 		driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
 
 		driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
+
+		driver.x().whileTrue(manipulator.runManipulator(() -> -0.2, true));
 		
 		driver.start().whileTrue(drivetrain.resetToVision());
 	
+		driver.povUp().whileTrue(
+			drivetrain.resetToVision().andThen(
+			drivetrain.goToPose(
+				() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][0], 
+				() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][1], 
+				() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
+			))
+		);
+
 		driver.rightBumper().whileTrue(
+				drivetrain.resetToVision().andThen(
 				drivetrain.goToPose(
 					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], 
 					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], 
-					() -> Units.degreesToRadians( 180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
+					() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
-				)
+				))
 			);
 
 		driver.leftBumper().whileTrue(
+			drivetrain.resetToVision().andThen(
 			drivetrain.goToPose(
 				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], 
 				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], 
-				() -> Units.degreesToRadians(180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
+				() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
-			)
+			))
 		);
 
 		//Operator inputs
@@ -202,10 +225,12 @@ public class RobotContainer {
 
 		operator.back().onTrue(elevator.homeCommand());
 
-		operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
+		operator.start().toggleOnTrue(
+			climberPivot.runPivot(() -> -operator.getRightY())
+			.alongWith(climberRollers.runRoller(() -> operator.getLeftY())));
 
 		operator.a().onTrue(
-			safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition)
+			safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
 		);
 
 		operator.x().onTrue(
@@ -225,16 +250,129 @@ public class RobotContainer {
 			.until(() -> driver.a().getAsBoolean())
 		);
 
+		operator.rightTrigger().onTrue(shootAlgae());
+
 	}
 	private void configureNamedCommands() {
-		new EventTrigger("Lift L4").whileTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		//new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
-		new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
 
-		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
+		NamedCommands.registerCommand(
-		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
+			"Drivetrain Set X", 
-		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).withTimeout(0.5));
+			drivetrain.setXCommand()
-		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		);
-		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
+
+		NamedCommands.registerCommand(
+			"Shoot Coral L4", 
+			Commands.race(
+				manipulator.runManipulator(
+					() -> 0.4, 
+					true
+				).withTimeout(
+					0.5
+				).andThen(
+					manipulator.runManipulator(
+						() -> 0.0, 
+						false
+					).withTimeout(
+						0.01
+					)
+				), 
+				Commands.parallel(
+					elevator.maintainPosition(), 
+					manipulatorPivot.maintainPosition()
+				)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Shoot Coral L4 Fast", 
+			Commands.race(
+				manipulator.runManipulator(
+					() -> 1,
+					true
+				).andThen(
+					manipulator.runManipulator(
+						() -> 1, 
+						true
+					).withTimeout(
+						0.125
+					)
+				).withTimeout(
+					3
+				).andThen(
+					manipulator.runManipulator(
+						() -> 0, 
+						true
+					)
+				), 
+				Commands.parallel(
+					elevator.maintainPosition(), 
+					manipulatorPivot.maintainPosition()
+				)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Collect Coral", 
+			manipulator.runUntilCollected(
+				() -> 0.30
+			).andThen(
+				manipulator.runManipulator(
+					() -> 0, 
+					false
+				).withTimeout(
+					0.01
+				)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Lift L4", 
+			safeMoveManipulator(
+				ElevatorConstants.kL4Position, 
+				ManipulatorPivotConstants.kL4Position
+			).andThen(
+				elevator.maintainPosition()
+					.withTimeout(
+						0.1
+					), 
+				manipulatorPivot.maintainPosition()
+					.withTimeout(
+						0.1
+					)
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"HP Pickup", 
+			safeMoveManipulator(
+				ElevatorConstants.kCoralIntakePosition, 
+				ManipulatorPivotConstants.kStartingPosition
+			)
+		);
+
+		NamedCommands.registerCommand(
+			"Shoot Algae",
+			shootAlgae().withTimeout(2)   
+		);
+
+		NamedCommands.registerCommand(
+			"Processor Position", 
+			moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
+			.raceWith(manipulator.runManipulator(() -> 0.85, false))
+		);
+
+		NamedCommands.registerCommand(
+			"Pickup Algae L2", 
+			moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
+			.raceWith(manipulator.runManipulator(() -> 0.85, false))
+			.andThen(
+				elevator.maintainPosition()
+				.alongWith(manipulatorPivot.maintainPosition())).withTimeout(0.1)
+
+			//Dont you need a holdPosition call?
+		);
 	}
 
 	//creates tabs and transforms them on the shuffleboard
@@ -253,7 +391,7 @@ public class RobotContainer {
 		sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(0, 0)
-			.withWidget(BuiltInWidgets.kGraph);
+			.withWidget(BuiltInWidgets.kTextView);
 
 		sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
 			.withSize(2, 1)
@@ -308,24 +446,46 @@ public class RobotContainer {
 
 		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
 
-		sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
-
-		sensorTab.addDouble("dt distance", drivetrain::driveDistance);
-
 		sensorTab.addDouble("velocity", drivetrain::getVelocity);
 
+		sensorTab.addDouble("heading", drivetrain::getHeading);
+
 
 		//sensorTab.add("odometry", drivetrain::getPose);
 
-		apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag());
+		apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
-		apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX());
+			.withSize(1,1).withPosition(1,1);
-		apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY());
+		apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
-		apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist());
+			.withSize(1,1).withPosition(2,1);
-		apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
+		apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
-		apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
+			.withSize(1,1).withPosition(3,1);
-		apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag());
+		apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
+			.withSize(1,1).withPosition(4,1);
+		apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
+			.withSize(1,1).withPosition(5,1);
+		apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
+			.withSize(1,1).withPosition(6,1);
 
-//		sensorTab.addDouble(" ID", vision::getOrangeClosestTag);
+		apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
+			.withSize(1,1).withPosition(1,2);
+		apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
+			.withSize(1,1).withPosition(2,2);
+		apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
+			.withSize(1,1).withPosition(3,2);
+		apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
+			.withSize(1,1).withPosition(4,2);
+		apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
+			.withSize(1,1).withPosition(5,2);
+		apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
+			.withSize(1,1).withPosition(6,2);
+
+		apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
+			.withSize(2,1).withPosition(4,4);
+
+		apriltagTab.addBoolean("Is orange connected?", () -> drivetrain.vision.isOrangeConnected())
+			.withSize(2, 1).withPosition(4, 2);
+		apriltagTab.addBoolean("Is black connected?", () -> drivetrain.vision.isBlackConnected())
+			.withSize(2, 1).withPosition(6, 2);
 
 
  	}
@@ -468,6 +628,14 @@ public class RobotContainer {
 		.raceWith(elevator.maintainPosition()));
 	}
 
+	private Command shootAlgae(){
+		return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
+		elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.raceWith(elevator.maintainPosition()));
+	}
+
 	@SuppressWarnings("unused")
 	private Command startingConfig() {
 		return moveManipulatorUtil(0, 0, false, true)

src/main/java/frc/robot/constants/AutoConstants.java

@@ -11,25 +11,32 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = 4;
+    public static final double kMaxSpeedMetersPerSecond = 5;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 2;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 4;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final double kPXController = 6;
+    public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
-    public static final double kPYController = 6;
+
-    public static final double kPThetaController = 5.5;
+    public static final double kPXYController = 3.5;
+    public static final double kPThetaController = 5;
+
+    public static final double kAlignPXYController = 2;
+    public static final double kAlignPThetaController = 5;
 
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
 
+    public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+
     // TODO This is a constant being managed like a static rewriteable variable
     public static RobotConfig kRobotConfig;
 
     public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
-        new PIDConstants(kPXController, 0, 0),
+        new PIDConstants(kPXYController, 0, 0),
-        new PIDConstants(kPYController, 0, 0)
+        new PIDConstants(kPThetaController, 0, 0)
     );
 
     static {

src/main/java/frc/robot/constants/ClimberRollersConstants.java

@@ -1,9 +1,18 @@
 package frc.robot.constants;
 
 import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 public class ClimberRollersConstants {
     public static final int kRollerMotorID = 9;
 
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+
+    static {
+        motorConfig
+            .smartCurrentLimit(40)
+            .idleMode(IdleMode.kBrake)
+            .inverted(true);
+        
+    }
 }

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 public class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 5.5;
+    public static final double kMaxSpeedMetersPerSecond = 5.5 * 0.75;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     // Chassis configuration
@@ -57,7 +57,7 @@ public class DrivetrainConstants {
 
     public static final boolean kGyroReversed = true;
 
-    public static final double kHeadingP = 0.01;
+    public static final double kHeadingP = 0.1;
 
     public static final double kXTranslationP = 0.5;
     public static final double kYTranslationP = 0.5;

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -42,7 +42,7 @@ public class ElevatorConstants {
     public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
 
     public static final double kCoralIntakePosition = 0;
-    public static final double kL1Position = 0;
+    public static final double kL1Position = 17;
     public static final double kL2Position = 11;
     public static final double kL3Position = 27;
     public static final double kL4Position = 50.5;
@@ -56,6 +56,8 @@ public class ElevatorConstants {
 
     public static final double kVoltageLimit = 7;
 
+    public static final double kVoltageLimitAlgae = 9;
+
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = .25;
     public static final double kSysIDStepVolts = 3;

src/main/java/frc/robot/constants/IndexerConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class IndexerConstants {
+    public static final int kIndexerMotorID = 16;
+
+    public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+    
+    static{
+        motorConfig.smartCurrentLimit(30)
+        .idleMode(IdleMode.kBrake);
+    };
+}

src/main/java/frc/robot/constants/ManipulatorConstants.java

@@ -1,5 +1,6 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ManipulatorConstants {
@@ -7,4 +8,9 @@ public class ManipulatorConstants {
     public static final int kCoralBeamBreakID = 2;
     
     public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
+    
+    static{
+        motorConfig.smartCurrentLimit(40)
+        .idleMode(IdleMode.kBrake);
+    };
 }

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -20,10 +20,12 @@ public class ManipulatorPivotConstants {
 
     public static final double kPivotMaxVelocity = 2 * Math.PI;
 
-    public static final double kPositionalP = 4;
+    public static final double kPositionalP = 4.5;
     public static final double kPositionalI = 0;
     public static final double kPositionalD = 0;
-    public static final double kPositionalTolerance = Units.degreesToRadians(1.5);
+    public static final double kPositionalTolerance = Units.degreesToRadians(3);
+
+    public static final double kAlgaeP = 7;
 
     public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
     public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
@@ -34,11 +36,11 @@ public class ManipulatorPivotConstants {
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
-    public static final double kEncoderOffset = 0.78-0.25;
+    public static final double kEncoderOffset = 0.04500000178813934;
 
     public static final double kStartingPosition = Units.degreesToRadians(90);
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
-    public static final double kL1Position = Units.degreesToRadians(0.0+90);
+    public static final double kL1Position = Units.degreesToRadians(246);
     public static final double kL2Position = Units.degreesToRadians(22.0+90);
     public static final double kL3Position = Units.degreesToRadians(22.0+90);
     public static final double kL4Position = Units.degreesToRadians(45.0+90);
@@ -48,6 +50,8 @@ public class ManipulatorPivotConstants {
     public static final double kNetPosition = Units.degreesToRadians(175.0+90);
     /**The closest position to the elevator brace without hitting it */
     public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
+
+    public static final double kBargeShotPosition = Units.degreesToRadians(222);
     /**The forward rotation limit of the pivot */
     public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
 

src/main/java/frc/robot/constants/OIConstants.java

@@ -9,4 +9,6 @@ public class OIConstants {
     public static final String kAutoTab = "Auto Tab";
     public static final String kSensorsTab = "Sensors Tab";
     public static final String kApriltagTab = "Apriltag Tab";
+
+    public static final double kJoystickExponential = 3;
 }

src/main/java/frc/robot/constants/VisionConstants.java

@@ -54,11 +54,37 @@ public class VisionConstants {
         {3.703, 3.975, 3.982, 2.806},
         {3.183, 4.191, 3.183, 3.857},
         {3.986, 5.24, 3.701, 5.076},
-        {5.275, 5.075, 4.991, 5.246},
+        {5.275, 5.075, 4.891, 5.284},//4.991, 5.246},
         {5.789, 3.862, 5.789, 4.194},
         {4.993, 2.816, 5.272, 2.996}
     };
 
-    public static final double latencyFudge = 0.080;
+    public static final double[][] algaeSetpointsMap = {
+        {},
+        {},
+        {},
+        {},
+        {},
+        {},
+        {13.906, 2.658},//6
+        {14.661, 4.013},
+        {13.834, 5.428},
+        {12.263, 5.452},
+        {11.412, 4.025},
+        {12.191, 2.574},//11
+        {},
+        {},
+        {},
+        {},
+        {},
+        {3.649, 2.558},//17
+        {2.776, 4.005},
+        {3.644, 5.514},
+        {5.296, 5.522},//4.991, 5.246},
+        {6.225, 4.008},
+        {5.322, 2.511}//22
+    };
+
+    public static final double latencyFudge = 0.0;
 
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -8,6 +8,7 @@ import java.io.File;
 import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -18,7 +19,9 @@ import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.HolonomicDriveController;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -59,13 +62,16 @@ public class Drivetrain extends SubsystemBase {
     public Orchestra m_orchestra = new Orchestra();
     private Timer musicTimer = new Timer();
 
-    private PIDController pidHeading;
+    private ProfiledPIDController pidHeading;
     private PIDController pidTranslationX;
     private PIDController pidTranslationY;
 
+    private HolonomicDriveController driveController;
+
     public Vision vision;
 
     public Pose2d orangePose2d;
+    public Pose2d blackPose2d;
 
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
@@ -104,19 +110,21 @@ public class Drivetrain extends SubsystemBase {
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
             },
-            new Pose2d()
+            new Pose2d(),
+            VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
+            VecBuilder.fill(1, 1, Units.degreesToRadians(360))
         );
 
-        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
+        pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
+        pidHeading.setTolerance(Units.degreesToRadians(3));
+        pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
 
-        pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
+        pidTranslationX = new PIDController(AutoConstants.kAlignPXYController,0,0);
-        pidHeading.enableContinuousInput(-180, 180);
-
-        pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
         pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
-        pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
+        pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0);
         pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
 
+        driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading);
 
         AutoBuilder.configure(
             this::getPose, 
@@ -152,6 +160,7 @@ public class Drivetrain extends SubsystemBase {
 
         vision = new Vision();
         orangePose2d = new Pose2d();
+        blackPose2d = new Pose2d();
     }
 
     @Override
@@ -166,14 +175,21 @@ public class Drivetrain extends SubsystemBase {
                 m_rearRight.getPosition()
         });
 
-        gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
+        gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
 
-       // if(vision.getOrangeDist() < 72){
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
-         //   m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
+
-        //}
+        if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
+            if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
+            }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
+            }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
+            }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
+            }
 
-        /* 
-        if(vision.getOrangeTagDetected()){
             // if the detected tags match your alliances reef tags use their pose estimates  
             if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
                 orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
@@ -182,29 +198,43 @@ public class Drivetrain extends SubsystemBase {
             }else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){
                 orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
                 m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
-            }
+            } 
-            Logger.recordOutput("orange pose", new Pose3d());
+        }
-            if(orangePose2d.equals(null)){
+        Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
-                Logger.recordOutput("orange pose", new Pose3d());
+        Logger.recordOutput("orange dist", vision.getOrangeDist());
-            }else{
+        Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
-                Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
+        Logger.recordOutput("orange tag", vision.getOrangeTagDetected());
+        Logger.recordOutput("orange FPS", vision.getOrangeFPS());
+
+
+        if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
+            if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
+            }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
+            }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
+            }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
             }
 
-            Logger.recordOutput("fused robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
+            if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
-        }
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp()); 
 
-        if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
+            }else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
-            Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
-            Logger.recordOutput("black pose", new Pose3d(blackPose));
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
-            m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp()); 
+            }
-
-        }else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
-            Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
-            Logger.recordOutput("black pose", new Pose3d(blackPose));
-            m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
         }
-            */
+        Logger.recordOutput("black pose", new Pose3d(blackPose2d));
-        
+        Logger.recordOutput("black dist", vision.getBlackDist());
+        Logger.recordOutput("black detected", vision.getBlackTagDetected());
+        Logger.recordOutput("black tag", vision.getBlackTagDetected());
+        Logger.recordOutput("black FPS", vision.getBlackFPS());
+
+        Logger.recordOutput("drive velocity", getVelocity());
+        Logger.recordOutput("closest tag", getClosestTag());
         Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
         
         if(musicTimer.get()>10){
@@ -249,7 +279,15 @@ public class Drivetrain extends SubsystemBase {
      * @param pose The pose to which to set the odometry.
      */
     public void resetOdometry(Pose2d pose) {
-        m_estimator.resetPose(
+
+        m_estimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroValue()),
+            new SwerveModulePosition[] {
+                m_frontLeft.getPosition(),
+                m_frontRight.getPosition(),
+                m_rearLeft.getPosition(),
+                m_rearRight.getPosition()
+            },
             pose
         );
     }
@@ -285,14 +323,24 @@ public class Drivetrain extends SubsystemBase {
      */
     public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
         // Convert the commanded speeds into the correct units for the drivetrain
-        double xSpeedDelivered = xSpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
-        double ySpeedDelivered = ySpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        double xSpeedDelivered = 0;
+        double ySpeedDelivered = 0;
+
+        if(p != 0){
+            xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+            ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        }else{
+            xSpeedDelivered = 0;
+            ySpeedDelivered = 0;
+        }
+        
         double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed;
 
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative
                 ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
-                        Rotation2d.fromDegrees(getGyroValue()))
+                        new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians()))
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -308,16 +356,33 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
-    public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
+    public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
-        return run(() -> {
+        return startRun(() -> {
-            drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.1, 0.1),
+                pidTranslationX.reset();
-                MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.1, 0.1), 
+                pidTranslationY.reset();
-                pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()), 
+                pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ());
-                true);
+            },
+            () -> {
+                    ChassisSpeeds controlEffort = driveController.calculate(
+                    m_estimator.getEstimatedPosition(),
+                    new Pose2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble(),
+                    headingSetpoint.get()),
+                    0,
+                    headingSetpoint.get()
+                );
+
+                double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond);
+                if (speed > AutoConstants.kMaxSpeedMetersPerSecondAutoAlign) {
+                    double mul = AutoConstants.kMaxSpeedMetersPerSecondAutoAlign / speed;
+                    controlEffort.vxMetersPerSecond *= mul;
+                    controlEffort.vyMetersPerSecond *= mul;
+                }
+
+                driveWithChassisSpeeds(controlEffort);
 
                 Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
                     new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
-                    new Rotation2d(headingSetpoint.getAsDouble()))));
+                    headingSetpoint.get())));
             });
     }
 
@@ -407,7 +472,7 @@ public class Drivetrain extends SubsystemBase {
      * @return the robot's heading in degrees, from -180 to 180
      */
     public double getHeading() {
-        return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
+        return m_estimator.getEstimatedPosition().getRotation().getDegrees();
     }
 
     public TimeInterpolatableBuffer<Double> getGyroBuffer(){
@@ -423,17 +488,17 @@ public class Drivetrain extends SubsystemBase {
         return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 
-    public double driveDistance(){
-        return m_frontLeft.getTotalDist();
-    }
-
     public double getVelocity(){
         return m_frontLeft.getState().speedMetersPerSecond;
     }
 
     public Command resetToVision(){
-        return run(() -> {
+        return runOnce(() -> {
-            m_estimator.resetPose(orangePose2d);
+            if(vision.getOrangeTagDetected()){
+                m_estimator.resetPose(new Pose2d(orangePose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
+            }else if(vision.getBlackTagDetected()){
+                m_estimator.resetPose(new Pose2d(blackPose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
+            }
         });
     }
 }

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

@@ -2,6 +2,8 @@ package frc.robot.subsystems;
 
 import java.util.function.DoubleSupplier;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
@@ -94,6 +96,10 @@ public class Elevator extends SubsystemBase {
         if (!getBottomLimitSwitch()) {
             encoder.setPosition(0);
         }
+
+        Logger.recordOutput("elevator position", getEncoderPosition());
+        Logger.recordOutput("elevator up setpoint", pidControllerUp.getSetpoint());
+        Logger.recordOutput("elevator down setpoint", pidControllerDown.getSetpoint());
     }
 
     /**
@@ -245,50 +251,65 @@ public class Elevator extends SubsystemBase {
                 
                 }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
         }
+    }          
+
+    public Command goToSetpointAlgae(DoubleSupplier setpoint) {
+            
+        if (setpoint.getAsDouble() == 0) {
+            return startRun(() -> {
         
-            
+                pidControllerUp.reset();
-            /*
+                pidControllerDown.reset();
-            elevatorMotor1.setVoltage(
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                pidController.calculate(
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
-                    encoder.getPosition(),
+                
-                    clampedSetpoint
+                }, 
-                ) + feedForward.calculate(0)
+                () -> {
-            );
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-*/
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-            /* 
+        
-            if (!pidController.atSetpoint()) {
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                elevatorMotor1.setVoltage(
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                    pidController.calculate(
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-                        encoder.getPosition(),
+                        );
-                        clampedSetpoint
+                    else{
-                    ) + feedForward.calculate(0)
+                        elevatorMotor1.setVoltage(
-                );
+                            MathUtil.clamp(
-            } else {
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-                elevatorMotor1.setVoltage(
+                        );
-                    feedForward.calculate(0)
+                    }
-                );
+                
-            }
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
-             
+                    .andThen(runManualElevator(() -> -.5)
-        });*/
+                    .until(() -> encoder.getPosition() == 0));
-    }
+
-            
+        } else {
-            /* 
+            return startRun(() -> {
-            if(encoder.getPosition() >= setpoint.getAsDouble()){
+        
-                elevatorMotor1.setVoltage(
+                pidControllerUp.reset();
-                    pidControllerUp.calculate(
+                pidControllerDown.reset();
-                        encoder.getPosition(),
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                        clampedSetpoint
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
-                    ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
+        
-                );
+                },
-            }else if(encoder.getPosition() <= setpoint.getAsDouble()){
+                () -> {
-                elevatorMotor1.setVoltage(
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-                    pidControllerDown.calculate(
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-                        encoder.getPosition(),
+        
-                        clampedSetpoint
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                    ) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                );
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
-            }
+                        );
-                */
+                    else{
+                        elevatorMotor1.setVoltage(
+                            MathUtil.clamp(
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
+                        );
+                    }
+                
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
+        }
+    }   
 
     /**
      * Returns the current encoder position

src/main/java/frc/robot/subsystems/MAXSwerveModule.java

@@ -144,7 +144,4 @@ public class MAXSwerveModule {
         m_drive.setPosition(0);
     }
 
-    public double getTotalDist(){
-        return m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters;
-    }
 }

src/main/java/frc/robot/subsystems/Manipulator.java

@@ -2,6 +2,8 @@ package frc.robot.subsystems;
 
 import java.util.function.DoubleSupplier;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -10,11 +12,14 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IndexerConstants;
 import frc.robot.constants.ManipulatorConstants;
 
 public class Manipulator extends SubsystemBase {
     private SparkMax manipulatorMotor;
 
+    private SparkMax indexerMotor;
+
     private DigitalInput coralBeamBreak;
 
     public Manipulator() {
@@ -29,9 +34,27 @@ public class Manipulator extends SubsystemBase {
             PersistMode.kPersistParameters
         );
 
+        indexerMotor = new SparkMax(
+            IndexerConstants.kIndexerMotorID,
+            MotorType.kBrushless
+        );
+
+        indexerMotor.configure(
+            IndexerConstants.motorConfig,
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
     }
 
+    @Override
+    public void periodic() {
+        super.periodic();
+
+        Logger.recordOutput("coral beam break", getCoralBeamBreak());
+    }
+
     /**
      * The default command for the manipulator that either stops the manipulator or slowly 
      * runs the manipulator to retain the algae
@@ -56,6 +79,8 @@ public class Manipulator extends SubsystemBase {
             manipulatorMotor.set(
                 coral ? speed.getAsDouble() : speed.getAsDouble() * -1
             );
+
+            indexerMotor.set(0);
         });
     }
 
@@ -68,15 +93,33 @@ public class Manipulator extends SubsystemBase {
      */
     public Command runUntilCollected(DoubleSupplier speed) {
         return run(() -> {
-            manipulatorMotor.set(
+            manipulatorMotor.setVoltage(
-                speed.getAsDouble()
+                speed.getAsDouble()*12
             );
-        }).until(() -> !coralBeamBreak.get());
+
+            indexerMotor.set(1);
+
+        }).unless(() -> !coralBeamBreak.get())
+        .until(() -> !coralBeamBreak.get());
+        /*
+        return run(() -> {
+            if(getCoralBeamBreak()) {
+                manipulatorMotor.set(
+                    speed.getAsDouble()
+                );
+            } else {
+                manipulatorMotor.set(
+                    speed.getAsDouble()
+                );
+            }
+        */
     }
 
     public Command retractCommand(DoubleSupplier retractSpeed){
         return run(() -> {
             manipulatorMotor.set(-retractSpeed.getAsDouble());
+
+            indexerMotor.set(0);
         }
         ).until(() -> coralBeamBreak.get());
     }

src/main/java/frc/robot/subsystems/ManipulatorPivot.java

@@ -4,12 +4,13 @@ import com.revrobotics.spark.SparkMax;
 
 import java.util.function.DoubleSupplier;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.spark.SparkAbsoluteEncoder;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
@@ -26,6 +27,8 @@ public class ManipulatorPivot extends SubsystemBase {
 
     private PIDController pidController;
 
+    private PIDController algaePIDController;
+
     public ManipulatorPivot() {
         pivotMotor = new SparkMax(
             ManipulatorPivotConstants.kPivotMotorID, 
@@ -45,10 +48,22 @@ public class ManipulatorPivot extends SubsystemBase {
             ManipulatorPivotConstants.kPositionalI, 
             ManipulatorPivotConstants.kPositionalD
         );
+        pidController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
+
         pidController.setSetpoint(0);
 
         pidController.enableContinuousInput(0, 280);
         
+        algaePIDController = new PIDController(
+            ManipulatorPivotConstants.kAlgaeP,
+            0,
+            0);
+        algaePIDController.setTolerance(ManipulatorPivotConstants.kPositionalTolerance);
+        
+        algaePIDController.setSetpoint(0);
+
+        algaePIDController.enableContinuousInput(0, 280);
+
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,
             ManipulatorPivotConstants.kFeedForwardG, 
@@ -56,6 +71,14 @@ public class ManipulatorPivot extends SubsystemBase {
             );
     }
 
+    @Override
+    public void periodic() {
+        super.periodic();
+
+        Logger.recordOutput("manipulator position", getEncoderPosition());
+        Logger.recordOutput("manipulator setpoint", pidController.getSetpoint());
+    }
+
     /**
      * Returns whether or not the motion is safe relative to the encoder's current position
      * and the arm safe stow position
@@ -126,6 +149,38 @@ public class ManipulatorPivot extends SubsystemBase {
             }).until(() -> pidController.atSetpoint());
     }
 
+    public Command goToSetpointAlgae(DoubleSupplier setpoint) {
+        return startRun(() -> {
+            
+            algaePIDController.setSetpoint(setpoint.getAsDouble());
+            algaePIDController.reset();
+            pidController.setSetpoint(setpoint.getAsDouble());
+            pidController.reset();
+            }, 
+            () -> {
+                /* 
+                if (!pidController.atSetpoint()) {
+                    pivotMotor.setVoltage(
+                        pidController.calculate(
+                            encoder.getPosition(),
+                            setpoint.getAsDouble()
+                        ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                } else {
+                    pivotMotor.setVoltage(
+                        -feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                    );
+                }
+                    */
+                pivotMotor.setVoltage(
+                    algaePIDController.calculate(
+                        encoder.getPosition(),
+                        setpoint.getAsDouble()
+                    ) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
+                );
+            }).until(() -> algaePIDController.atSetpoint());
+    }
+
     public Command maintainPosition() {
         return startRun(() -> {
             

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

@@ -15,6 +15,8 @@ import frc.robot.constants.VisionConstants;
 
 public class Vision{
 
+    private NetworkTable blackVisionTable;
+
     private DoubleSubscriber black_tx;
     private DoubleSubscriber black_ty;
     private DoubleSubscriber black_dist;
@@ -24,6 +26,8 @@ public class Vision{
 
     private DoubleSubscriber blackFramerate;
 
+    private NetworkTable orangeVisionTable;
+
     private DoubleSubscriber orange_tx;
     private DoubleSubscriber orange_ty;
     private DoubleSubscriber orange_dist;
@@ -39,8 +43,8 @@ public class Vision{
     public Vision(){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
-        NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
+        blackVisionTable = inst.getTable("black_Fiducial");
-        NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
+        orangeVisionTable = inst.getTable("orange_Fiducial");
 
         black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
         black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
@@ -80,9 +84,9 @@ public class Vision{
 
        // System.out.println(gyroBuffer.getSample(timestamp));
 
-        double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-Units.degreesToRadians( camPose[1]) + Units.degreesToRadians(ty));
+        double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-camPose[1] + Units.degreesToRadians(ty));
 
-        Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(Units.degreesToRadians(camPose[2])).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
+        Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
 
         Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
         Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
@@ -93,8 +97,8 @@ public class Vision{
             .getTranslation();
         Pose2d robotPose = new Pose2d(
             fieldToCameraTranslation,
-            new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(Units.degreesToRadians( camPose[2]))))
+            new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(camPose[2])))
-            .transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(Units.degreesToRadians(camPose[2]))), Pose2d.kZero));
+            .transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(camPose[2])), Pose2d.kZero));
 
         robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));
 
@@ -102,8 +106,8 @@ public class Vision{
     }
 
     public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
-        return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
+        return cameraToGlobalPose2d(getBlackClosestTag(), black_dist.get(),
-            getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose);
+            getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
     }
 
     public double getBlackTX(){
@@ -123,7 +127,7 @@ public class Vision{
     }
 
     public double getBlackTimeStamp(){
-        return black_tx.getLastChange();
+        return black_tx.getLastChange()-VisionConstants.latencyFudge;
     }
 
     public boolean getBlackTagDetected(){
@@ -137,7 +141,7 @@ public class Vision{
     public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
         if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
             return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
-                orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose
+                orange_tx.get(), orange_ty.get(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose
                 );
         }else{
             return new Pose2d();
@@ -171,5 +175,13 @@ public class Vision{
     public double getOrangeFPS(){
         return orangeFramerate.get();
     }
+
+    public boolean isBlackConnected(){
+        return Timer.getFPGATimestamp()-blackFramerate.getLastChange() > 3.0;
+    }
+
+    public boolean isOrangeConnected(){
+        return Timer.getFPGATimestamp()-orangeFramerate.getLastChange() > 3.0;
+    }
     
 }