no worky

on controller elevator PID control
in the midst of making controller based pid control
2025-05-20 16:47:48 -04:00 · 2025-05-19 22:49:28 -04:00 · 2025-05-19 18:16:57 -04:00 · 2025-05-19 01:55:28 -04:00 · 2025-05-19 01:48:50 -04:00 · 2025-05-17 13:48:42 -04:00
39 changed files with 1041 additions and 535 deletions
--- a/src/main/deploy/pathplanner/autos/1
+++ b/src/main/deploy/pathplanner/autos/1
@@ -27,6 +27,12 @@
          "data": {
            "pathName": "H Backup"
          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "HP Pickup"
+          }
        }
      ]
    }
--- a/src/main/deploy/pathplanner/autos/1L4
+++ b/src/main/deploy/pathplanner/autos/1L4
@@ -7,7 +7,7 @@
        {
          "type": "path",
          "data": {
-            "pathName": "Start to 30 Right"
+            "pathName": "Start to H"
          }
        },
        {
@@ -16,54 +16,54 @@
            "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": "path",
-                "data": {
-                  "pathName": "30 Right to HP"
-                }
-              },
              {
                "type": "named",
                "data": {
                  "name": "HP Pickup"
                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Post-Barge Backup"
+                }
              }
            ]
          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Collect Coral"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "HP to 330 Left"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Lift L4"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "K Approach"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Shoot Coral L4"
-          }
        }
      ]
    }
--- a/src/main/deploy/pathplanner/autos/2.5
+++ b/src/main/deploy/pathplanner/autos/2.5
@@ -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
+}
--- a/src/main/deploy/pathplanner/autos/Two
+++ b/src/main/deploy/pathplanner/autos/Two
@@ -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"
          }
        },
        {
--- a/src/main/deploy/pathplanner/autos/Two
+++ b/src/main/deploy/pathplanner/autos/Two
@@ -1,49 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Right Start to E"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Shoot Coral L4"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "HP Pickup"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "E to HP"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Collect Coral"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "HP to D"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}
--- a/src/main/deploy/pathplanner/paths/30
+++ b/src/main/deploy/pathplanner/paths/30
@@ -8,20 +8,20 @@
      },
      "prevControl": null,
      "nextControl": {
-        "x": 5.649657534252619,
-        "y": 6.474186643842743
+        "x": 4.843032786885245,
+        "y": 6.30266393442623
      },
      "isLocked": false,
      "linkedName": null
    },
    {
      "anchor": {
-        "x": 1.1987704918032787,
-        "y": 7.189754098360656
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
      },
      "prevControl": {
-        "x": 2.055234532899169,
-        "y": 6.243135947675724
+        "x": 2.287270519874242,
+        "y": 6.774371912194027
      },
      "nextControl": null,
      "isLocked": false,
@@ -33,8 +33,8 @@
  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 5.0,
+    "maxAcceleration": 2.5,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 400.0,
    "nominalVoltage": 12.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
 }
--- a/src/main/deploy/pathplanner/paths/330
+++ b/src/main/deploy/pathplanner/paths/330
@@ -45,8 +45,8 @@
    }
  ],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 5.0,
+    "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
 }
--- a/src/main/deploy/pathplanner/paths/E
+++ b/src/main/deploy/pathplanner/paths/E
@@ -52,8 +52,8 @@
    }
  ],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 400.0,
    "nominalVoltage": 12.0,
--- a/src/main/deploy/pathplanner/paths/H
+++ b/src/main/deploy/pathplanner/paths/H
@@ -3,29 +3,29 @@
  "waypoints": [
    {
      "anchor": {
-        "x": 5.7,
-        "y": 4.3
+        "x": 5.758260140458621,
+        "y": 4.193633481772718
      },
      "prevControl": null,
      "nextControl": {
-        "x": 6.053790983606557,
-        "y": 4.312704918032787
+        "x": 6.112051124065178,
+        "y": 4.206338399805505
      },
      "isLocked": false,
      "linkedName": "H"
    },
    {
      "anchor": {
-        "x": 6.389446721311475,
-        "y": 4.3
+        "x": 6.5,
+        "y": 4.021985060730393
      },
      "prevControl": {
-        "x": 6.1394487099079695,
-        "y": 4.300997143065429
+        "x": 6.250001988596495,
+        "y": 4.022982203795822
      },
      "nextControl": null,
      "isLocked": false,
-      "linkedName": null
+      "linkedName": "Behind HG Face"
    }
  ],
  "rotationTargets": [],
@@ -33,8 +33,8 @@
  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 3.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
 }
--- a/src/main/deploy/pathplanner/paths/HG
+++ b/src/main/deploy/pathplanner/paths/HG
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.5,
+        "y": 4.021985060730393
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.224227344787433,
+        "y": 4.009974315068493
+      },
+      "isLocked": false,
+      "linkedName": "Behind HG Face"
+    },
+    {
+      "anchor": {
+        "x": 5.872671261177789,
+        "y": 4.021985060730393
+      },
+      "prevControl": {
+        "x": 6.125859753477119,
+        "y": 4.004837296105254
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HG Algae"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}
--- a/src/main/deploy/pathplanner/paths/HG
+++ b/src/main/deploy/pathplanner/paths/HG
@@ -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
+}
--- a/src/main/deploy/pathplanner/paths/HP
+++ b/src/main/deploy/pathplanner/paths/HP
@@ -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
+}
--- a/src/main/deploy/pathplanner/paths/HP
+++ b/src/main/deploy/pathplanner/paths/HP
@@ -52,8 +52,8 @@
    }
  ],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 400.0,
    "nominalVoltage": 12.0,
--- a/src/main/deploy/pathplanner/paths/HP
+++ b/src/main/deploy/pathplanner/paths/HP
@@ -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
+}
--- a/src/main/deploy/pathplanner/paths/J
+++ b/src/main/deploy/pathplanner/paths/J
@@ -33,8 +33,8 @@
  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 400.0,
    "nominalVoltage": 12.0,
--- a/src/main/deploy/pathplanner/paths/J
+++ b/src/main/deploy/pathplanner/paths/J
@@ -33,8 +33,8 @@
  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 400.0,
    "nominalVoltage": 12.0,
--- a/src/main/deploy/pathplanner/paths/K
+++ b/src/main/deploy/pathplanner/paths/K
@@ -3,25 +3,25 @@
  "waypoints": [
    {
      "anchor": {
-        "x": 3.6202868852459016,
-        "y": 5.859118852459017
+        "x": 3.35655737704918,
+        "y": 5.78719262295082
      },
      "prevControl": null,
      "nextControl": {
-        "x": 3.6663590190101236,
-        "y": 5.6134008057257505
+        "x": 3.402629510813402,
+        "y": 5.541474576217554
      },
      "isLocked": false,
      "linkedName": "Before K"
    },
    {
      "anchor": {
-        "x": 3.7290765411991873,
-        "y": 5.091647695859483
+        "x": 3.6322745901639335,
+        "y": 5.019979508196721
      },
      "prevControl": {
-        "x": 3.693113426445089,
-        "y": 5.343389499138171
+        "x": 3.596311475409835,
+        "y": 5.271721311475409
      },
      "nextControl": null,
      "isLocked": false,
@@ -33,8 +33,8 @@
  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 1.0,
+    "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
 }
--- a/src/main/deploy/pathplanner/paths/HP
+++ b/src/main/deploy/pathplanner/paths/HP
@@ -3,29 +3,29 @@
  "waypoints": [
    {
      "anchor": {
-        "x": 1.1987704918032787,
-        "y": 7.189754098360656
+        "x": 3.609900518622585,
+        "y": 5.005924534374863
      },
      "prevControl": null,
      "nextControl": {
-        "x": 2.490979395912868,
-        "y": 6.964368824388053
+        "x": 3.47740078362179,
+        "y": 5.217924110376135
      },
      "isLocked": false,
-      "linkedName": "HP Left Position"
+      "linkedName": "L"
    },
    {
      "anchor": {
-        "x": 3.6202868852459016,
-        "y": 5.859118852459017
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
      },
      "prevControl": {
-        "x": 3.304747501684258,
-        "y": 6.385017825061756
+        "x": 1.3306352459016395,
+        "y": 6.926024590163935
      },
      "nextControl": null,
      "isLocked": false,
-      "linkedName": "Before K"
+      "linkedName": "HP Left Position"
    }
  ],
  "rotationTargets": [],
@@ -33,22 +33,22 @@
  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 5.0,
+    "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
 }
--- a/src/main/deploy/pathplanner/paths/New
+++ b/src/main/deploy/pathplanner/paths/New
@@ -16,12 +16,12 @@
    },
    {
      "anchor": {
-        "x": 12.272,
-        "y": 2.975
+        "x": 3.756421232876712,
+        "y": 5.227054794520548
      },
      "prevControl": {
-        "x": 10.940715672291898,
-        "y": 2.975
+        "x": 2.756421232876712,
+        "y": 5.227054794520548
      },
      "nextControl": null,
      "isLocked": false,
@@ -30,22 +30,11 @@
  ],
  "rotationTargets": [],
  "constraintZones": [],
-  "pointTowardsZones": [
-    {
-      "fieldPosition": {
-        "x": 0.4,
-        "y": 5.5
-      },
-      "rotationOffset": 0.0,
-      "minWaypointRelativePos": 0.15,
-      "maxWaypointRelativePos": 0.4,
-      "name": "Point Towards Zone"
-    }
-  ],
+  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 3.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
--- a/src/main/deploy/pathplanner/paths/Post-Barge
+++ b/src/main/deploy/pathplanner/paths/Post-Barge
@@ -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
+}
--- a/src/main/deploy/pathplanner/paths/Right
+++ b/src/main/deploy/pathplanner/paths/Right
@@ -52,8 +52,8 @@
    }
  ],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 400.0,
    "nominalVoltage": 12.0,
--- a/src/main/deploy/pathplanner/paths/Start
+++ b/src/main/deploy/pathplanner/paths/Start
@@ -3,13 +3,13 @@
  "waypoints": [
    {
      "anchor": {
-        "x": 7.1686475409836055,
-        "y": 7.573360655737705
+        "x": 7.150967037968244,
+        "y": 7.5521014571037055
      },
      "prevControl": null,
      "nextControl": {
-        "x": 5.933913934426228,
-        "y": 6.5544057377049185
+        "x": 5.916233431410867,
+        "y": 6.533146539070919
      },
      "isLocked": false,
      "linkedName": null
@@ -45,8 +45,8 @@
    }
  ],
  "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.25,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 400.0,
    "nominalVoltage": 12.0,
@@ -57,10 +57,10 @@
    "rotation": -119.71497744813712
  },
  "reversed": false,
-  "folder": null,
+  "folder": "Left Paths",
  "idealStartingState": {
    "velocity": 0,
    "rotation": -90.0
  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
--- a/src/main/deploy/pathplanner/paths/Start
+++ b/src/main/deploy/pathplanner/paths/Start
@@ -16,12 +16,12 @@
    },
    {
      "anchor": {
-        "x": 5.7,
-        "y": 4.3
+        "x": 5.758260140458621,
+        "y": 4.193633481772718
      },
      "prevControl": {
-        "x": 6.347336065573771,
-        "y": 4.2640368852459005
+        "x": 6.405596206032391,
+        "y": 4.157670367018619
      },
      "nextControl": null,
      "isLocked": false,
@@ -31,9 +31,21 @@
  "rotationTargets": [],
  "constraintZones": [],
  "pointTowardsZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.10238095238095252,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
  "globalConstraints": {
-    "maxVelocity": 2.0,
+    "maxVelocity": 3.5,
    "maxAcceleration": 1.0,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 400.0,
@@ -45,10 +57,10 @@
    "rotation": 180.0
  },
  "reversed": false,
-  "folder": null,
+  "folder": "Center",
  "idealStartingState": {
    "velocity": 0,
    "rotation": 180.0
  },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
--- a/src/main/deploy/pathplanner/paths/fein.path
+++ b/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": 2.0,
-    "maxAcceleration": 1.0,
-    "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
-}
--- a/src/main/deploy/pathplanner/settings.json
+++ b/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": 2.0,
-  "defaultMaxAccel": 1.0,
+  "defaultMaxVel": 3.5,
+  "defaultMaxAccel": 1.75,
  "defaultMaxAngVel": 540.0,
  "defaultMaxAngAccel": 400.0,
  "defaultNominalVoltage": 12.0,
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/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,8 +20,7 @@ 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.path.EventMarker;
+import com.pathplanner.lib.commands.PathPlannerAuto;

 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
@@ -33,9 +30,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;

 public class RobotContainer {
 	private ClimberPivot climberPivot;
@@ -47,6 +42,8 @@ public class RobotContainer {
 	private Elevator elevator;
 	//private ElevatorSysID elevator;

+	//private Indexer indexer;
+
 	private Manipulator manipulator;

 	private ManipulatorPivot manipulatorPivot;
@@ -68,6 +65,8 @@ public class RobotContainer {
 		elevator = new Elevator();
 		//elevator = new ElevatorSysID();

+		//indexer = new Indexer();
+
 		manipulator = new Manipulator();

 		manipulatorPivot = new ManipulatorPivot();
@@ -78,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;

@@ -109,9 +111,9 @@ public class RobotContainer {

 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
-				() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
-				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
+				() -> Math.pow(driver.getLeftY(), 3),
+				() -> Math.pow(driver.getLeftX(), 3),
+				() -> driver.getRightX(), 
 				() -> true
 			)
 		);
@@ -120,6 +122,10 @@ public class RobotContainer {
 			elevator.maintainPosition()
 		);

+		//indexer.setDefaultCommand(
+		//	indexer.runIndexer(() -> 0)
+		//);
+		
 		
 		manipulatorPivot.setDefaultCommand(
 			manipulatorPivot.maintainPosition()
@@ -142,6 +148,7 @@ public class RobotContainer {

 		driver.leftTrigger().whileTrue( 
 			manipulator.runUntilCollected(() -> 0.75)
+			//.alongWith(indexer.runIndexer(() -> .75))
 			.until(() -> manipulator.getCoralBeamBreak() == false)
 			.andThen(manipulator.retractCommand(() -> .1))
 		);
@@ -156,8 +163,19 @@ public class RobotContainer {

 		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(
@@ -207,7 +225,9 @@ 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.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)
@@ -237,12 +257,122 @@ public class RobotContainer {
 		//new EventTrigger("Lift L4").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("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(1).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.1)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
-		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).until(() -> manipulator.getCoralBeamBreak() == false).andThen(manipulator.runManipulator(() -> 0, false)).withTimeout(0.1));
-		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(
+			"Drivetrain Set X", 
+			drivetrain.setXCommand()
+		);
+
+		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
@@ -261,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)
@@ -294,32 +424,8 @@ public class RobotContainer {
 		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
 			.withWidget(BuiltInWidgets.kGraph);

-		sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
-			.withSize(1, 1)
-			.withPosition(5, 0)
-			.withWidget(BuiltInWidgets.kTextView);
-
-		sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
-			.withSize(1, 1)
-			.withPosition(5, 1)
-			.withWidget(BuiltInWidgets.kTextView);
-
-			sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
-			.withSize(1, 1)
-			.withPosition(5, 0)
-			.withWidget(BuiltInWidgets.kTextView);
-
-		sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
-			.withSize(1, 1)
-			.withPosition(5, 1)
-			.withWidget(BuiltInWidgets.kTextView);
-
 		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);
@@ -330,15 +436,15 @@ public class RobotContainer {
 		apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
 			.withSize(1,1).withPosition(1,1);
 		apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
-			.withSize(2,1).withPosition(2,1);
+			.withSize(1,1).withPosition(2,1);
 		apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
-			.withSize(3,1).withPosition(3,1);
+			.withSize(1,1).withPosition(3,1);
 		apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
-			.withSize(4,1).withPosition(4,1);
+			.withSize(1,1).withPosition(4,1);
 		apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
-			.withSize(6,1).withPosition(5,1);
+			.withSize(1,1).withPosition(5,1);
 		apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
-			.withSize(7,1).withPosition(6,1);
+			.withSize(1,1).withPosition(6,1);

 		apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
 			.withSize(1,1).withPosition(1,2);
@@ -354,7 +460,13 @@ public class RobotContainer {
 			.withSize(1,1).withPosition(6,2);

 		apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
-			.withSize(1,1).withPosition(4,4);
+			.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);
+

 	}

@@ -497,10 +609,10 @@ public class RobotContainer {
 	}

 	private Command shootAlgae(){
-		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>44).andThen(manipulator.runManipulator(() -> -1, false),
-		elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		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()));
 	}

--- a/src/main/java/frc/robot/constants/AutoConstants.java
+++ b/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 = 5.5;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+    public static final double kMaxSpeedMetersPerSecond = 5;
+    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 = 3;
-    public static final double kPYController = 3;
-    public static final double kPThetaController = 5.5;
+    public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.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(kPYController, 0, 0)
+        new PIDConstants(kPXYController, 0, 0),
+        new PIDConstants(kPThetaController, 0, 0)
    );

    static {
--- a/src/main/java/frc/robot/constants/ClimberRollersConstants.java
+++ b/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);
+        
+    }
 }
--- a/src/main/java/frc/robot/constants/DrivetrainConstants.java
+++ b/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
--- a/src/main/java/frc/robot/constants/ElevatorConstants.java
+++ b/src/main/java/frc/robot/constants/ElevatorConstants.java
@@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;

 import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;

 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -22,6 +23,7 @@ public class ElevatorConstants {

    public static final int kCurrentLimit = 40;

+    /*
    public static final double kUpControllerP = 5.6;//7; //
    public static final double kUpControllerI = 0;
    public static final double kUpControllerD = 0.28;//0.28
@@ -31,6 +33,11 @@ public class ElevatorConstants {
    public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35

    public static final double kMaintainP = 3;
+    */
+
+    public static final double kP = 3;//7; //
+    public static final double kI = 0;
+    public static final double kD = 0;//.28;//0.28 

    public static final double kAllowedError = 1;

@@ -38,11 +45,13 @@ public class ElevatorConstants {
    public static final double kFeedForwardG = (0.95 + 0.2)/2;  /* kG too high + kG too low / 2 */    // calculated value 0.6
    public static final double kFeedForwardV = 0.12;   // calculated value 0.12

-    public static final double kMaxVelocity = 150.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
-    public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    public static final double kMaxVelocity = 100.0;    // 100 inches per second (COOKING) calculated max is 184 in/s
+    public static final double kMaxAcceleration = 50;    // 50 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    public static final double kMaxVelocityAlgae = 120;
+    public static final double kMaxAccelerationAlgae = 400;

    public static final double kCoralIntakePosition = 0;
-    public static final double kL1Position = 14;
+    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 +65,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;
@@ -81,5 +92,9 @@ public class ElevatorConstants {
        motorConfig.encoder
            .positionConversionFactor(kEncoderPositionConversionFactor)
            .velocityConversionFactor(kEncoderVelocityConversionFactor);
+        motorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1);
    }
 }
--- a/src/main/java/frc/robot/constants/IndexerConstants.java
+++ b/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);
+    };
+}
--- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
+++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
@@ -23,7 +23,9 @@ public class ManipulatorPivotConstants {
    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,7 +36,7 @@ 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);
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -59,6 +59,32 @@ public class VisionConstants {
        {4.993, 2.816, 5.272, 2.996}
    };

+    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;

 }
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -14,14 +14,14 @@ import org.littletonrobotics.junction.Logger;

 import com.ctre.phoenix6.Orchestra;
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.RotationTarget;
 import com.studica.frc.AHRS;
 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.estimator.PoseEstimator;
+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;
@@ -62,10 +62,12 @@ 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;
@@ -113,15 +115,16 @@ public class Drivetrain extends SubsystemBase {
            VecBuilder.fill(1, 1, Units.degreesToRadians(360))
        );

-        pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
+        pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
        pidHeading.setTolerance(Units.degreesToRadians(3));
        pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));

-        pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
+        pidTranslationX = new PIDController(AutoConstants.kAlignPXYController,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, 
@@ -174,11 +177,17 @@ public class Drivetrain extends SubsystemBase {

        gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());

-        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));

-        if(vision.getOrangeTagDetected()){
-            if(vision.getOrangeDist() < 60){
+        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 the detected tags match your alliances reef tags use their pose estimates  
@@ -192,11 +201,21 @@ public class Drivetrain extends SubsystemBase {
            } 
        }
        Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
+        Logger.recordOutput("orange dist", vision.getOrangeDist());
+        Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
+        Logger.recordOutput("orange tag", vision.getOrangeTagDetected());
+        Logger.recordOutput("orange FPS", vision.getOrangeFPS());


-        if(vision.getBlackTagDetected()){
-            if(vision.getBlackDist() < 60){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360)));
+        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)));
            }

            if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
@@ -209,7 +228,13 @@ public class Drivetrain extends SubsystemBase {
            }
        }
        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){
@@ -323,16 +348,27 @@ public class Drivetrain extends SubsystemBase {

    public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
        return startRun(() -> {
-            pidHeading.reset();
-            pidTranslationX.reset();
-            pidTranslationY.reset();
+                pidTranslationX.reset();
+                pidTranslationY.reset();
+                pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ());
            },
            () -> {
-            drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.05, 0.05),
-                MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.05, 0.05), 
-                pidHeading.calculate(Units.degreesToRadians(getHeading()), headingSetpoint.get().getRadians()), 
-                true);
-                Logger.recordOutput("reeeee", pidHeading.getError());
+                    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()),
@@ -442,17 +478,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 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()));
+            }
        });
    }
 }
--- a/src/main/java/frc/robot/subsystems/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -2,15 +2,20 @@ package frc.robot.subsystems;

 import java.util.function.DoubleSupplier;

+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
 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.ElevatorFeedforward;
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -24,13 +29,15 @@ public class Elevator extends SubsystemBase {

    private DigitalInput bottomLimitSwitch;

-    private PIDController pidControllerUp;
-    private PIDController pidControllerDown;
-
-    private PIDController maintainPID;
-
    private ElevatorFeedforward feedForward;

+    private TrapezoidProfile trapProfile;
+    private TrapezoidProfile trapProfileAlgae;
+    private TrapezoidProfile.State goal;
+    private TrapezoidProfile.State setpoint;
+
+    private SparkClosedLoopController controller;
+    
    public Elevator() {
        elevatorMotor1 = new SparkMax(
            ElevatorConstants.kElevatorMotor1ID, 
@@ -60,33 +67,17 @@ public class Elevator extends SubsystemBase {
            ElevatorConstants.kBottomLimitSwitchID
        );

-        pidControllerDown = new PIDController(
-            ElevatorConstants.kDownControllerP, 
-            ElevatorConstants.kDownControllerI,
-            ElevatorConstants.kDownControllerD
-        );
-        pidControllerDown.setSetpoint(0);
-
-        pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
-
-        pidControllerUp = new PIDController(
-            ElevatorConstants.kUpControllerP, 
-            ElevatorConstants.kUpControllerI,
-            ElevatorConstants.kUpControllerD
-        );
-        pidControllerUp.setSetpoint(0);
-
-        pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
-
-        maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
-
-        maintainPID.setTolerance(ElevatorConstants.kAllowedError);
-
        feedForward = new ElevatorFeedforward(
            ElevatorConstants.kFeedForwardS, 
            ElevatorConstants.kFeedForwardG, 
            ElevatorConstants.kFeedForwardV
        );
+
+        trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration));
+
+        trapProfileAlgae = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocityAlgae, ElevatorConstants.kMaxAccelerationAlgae));
+
+        controller = elevatorMotor1.getClosedLoopController();
    }

    @Override
@@ -94,6 +85,8 @@ public class Elevator extends SubsystemBase {
        if (!getBottomLimitSwitch()) {
            encoder.setPosition(0);
        }
+
+        Logger.recordOutput("elevator position", getEncoderPosition());
    }

    /**
@@ -147,28 +140,15 @@ public class Elevator extends SubsystemBase {
    public Command maintainPosition() {

        return startRun(() -> {
-            maintainPID.reset();
-            maintainPID.setSetpoint(pidControllerUp.getSetpoint());
+
        }, 
        () -> {
-
-            double maintainOutput = maintainPID.calculate(getEncoderPosition());
-
-            if(!maintainPID.atSetpoint()) 
-                elevatorMotor1.setVoltage( MathUtil.clamp(
-                    maintainOutput + feedForward.calculate(0), -2, 2)
-                );
-            else{
-                elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-            } 
-            
-/* 
-            elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-                */
+            controller.setReference(
+                encoder.getPosition(), 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(0.0)
+            );

        });
        
@@ -186,109 +166,42 @@ public class Elevator extends SubsystemBase {
     * Moves the elevator to a target destination (setpoint). 
     * 
     * @param setpoint Target destination of the subsystem
-     * @param timeout Time to achieve the setpoint before quitting
     * @return Sets motor voltage to achieve the target destination
     */
-    public Command goToSetpoint(DoubleSupplier setpoint) {
+    public Command goToSetpoint(DoubleSupplier setGoal) {
        
-        if (setpoint.getAsDouble() == 0) {
-            return startRun(() -> {
+        return startRun(() -> {
+            goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
+        }, () -> {
+            setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);

-                pidControllerUp.reset();
-                pidControllerDown.reset();
-                pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                pidControllerDown.setSetpoint(setpoint.getAsDouble());
-                
-                }, 
-                () -> {
-                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-        
-                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    }
-                
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
-                    .andThen(runManualElevator(() -> -.5)
-                    .until(() -> encoder.getPosition() == 0));
-
-        } else {
-            return startRun(() -> {
-        
-                pidControllerUp.reset();
-                pidControllerDown.reset();
-                pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                pidControllerDown.setSetpoint(setpoint.getAsDouble());
-        
-                },
-                () -> {
-                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-        
-                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    }
-                
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
-        }
-        
-            
-            /*
-            elevatorMotor1.setVoltage(
-                pidController.calculate(
-                    encoder.getPosition(),
-                    clampedSetpoint
-                ) + feedForward.calculate(0)
+            controller.setReference(
+                setpoint.position, 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(encoder.getVelocity())
            );
-*/
-            /* 
-            if (!pidController.atSetpoint()) {
-                elevatorMotor1.setVoltage(
-                    pidController.calculate(
-                        encoder.getPosition(),
-                        clampedSetpoint
-                    ) + feedForward.calculate(0)
-                );
-            } else {
-                elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-            }

-        });*/
+        }).until(() -> trapProfile.isFinished(encoder.getPosition()));
+
    }          

-            /* 
-            if(encoder.getPosition() >= setpoint.getAsDouble()){
-                elevatorMotor1.setVoltage(
-                    pidControllerUp.calculate(
-                        encoder.getPosition(),
-                        clampedSetpoint
-                    ) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
-                );
-            }else if(encoder.getPosition() <= setpoint.getAsDouble()){
-                elevatorMotor1.setVoltage(
-                    pidControllerDown.calculate(
-                        encoder.getPosition(),
-                        clampedSetpoint
-                    ) + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
-                );
-            }
-                */
+    public Command goToSetpointAlgae(DoubleSupplier setGoal) {
+            
+        return startRun(() -> {
+            goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
+        }, () -> {
+            setpoint = trapProfileAlgae.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
+
+            controller.setReference(
+                setpoint.position, 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(setpoint.velocity)
+            );
+
+        }).until(() -> trapProfileAlgae.isFinished(encoder.getPosition()));
+    }   

    /**
     * Returns the current encoder position
@@ -327,19 +240,4 @@ public class Elevator extends SubsystemBase {
        return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
    }

-    public double getPIDUpSetpoint() {
-        return pidControllerUp.getSetpoint();
-    }
-
-    public double getPIDUpError() {
-        return pidControllerUp.getError();
-    }
-
-    public double getPIDDownSetpoint() {
-        return pidControllerDown.getSetpoint();
-    }
-
-    public double getPIDDownError() {
-        return pidControllerDown.getError();
-    }
 }
--- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java
+++ b/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;
-    }
 }
--- a/src/main/java/frc/robot/subsystems/Manipulator.java
+++ b/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,9 +93,12 @@ public class Manipulator extends SubsystemBase {
     */
    public Command runUntilCollected(DoubleSupplier speed) {
        return run(() -> {
-            manipulatorMotor.set(
-                speed.getAsDouble()
+            manipulatorMotor.setVoltage(
+                speed.getAsDouble()*12
            );
+
+            indexerMotor.set(1);
+
        }).unless(() -> !coralBeamBreak.get())
        .until(() -> !coralBeamBreak.get());
        /*
@@ -90,6 +118,8 @@ public class Manipulator extends SubsystemBase {
    public Command retractCommand(DoubleSupplier retractSpeed){
        return run(() -> {
            manipulatorMotor.set(-retractSpeed.getAsDouble());
+
+            indexerMotor.set(0);
        }
        ).until(() -> coralBeamBreak.get());
    }
--- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java
+++ b/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(() -> {
            
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -176,4 +176,12 @@ public class Vision{
        return orangeFramerate.get();
    }

+    public boolean isBlackConnected(){
+        return Timer.getFPGATimestamp()-blackFramerate.getLastChange() > 3.0;
+    }
+
+    public boolean isOrangeConnected(){
+        return Timer.getFPGATimestamp()-orangeFramerate.getLastChange() > 3.0;
+    }
+    
 }
Author	SHA1	Message	Date
Tyler-j42	ba7e8d59ad	no worky	2025-05-20 16:47:48 -04:00
Tylr-J42	626b92b769	on controller elevator PID control	2025-05-19 22:49:28 -04:00
Tylr-J42	42d47d6075	in the midst of making controller based pid control	2025-05-19 18:16:57 -04:00
Tylr-J42	68da3c630c	Noah WTF are you smoking? (fixed driver bindings)	2025-05-19 01:55:28 -04:00
Team 2648	c9316cebc3	end of mayhem	2025-05-19 01:48:50 -04:00
Team 2648	d312e125cd	before mayhem elims	2025-05-17 13:48:42 -04:00
Team 2648	4386de4d4d	After champs code	2025-04-22 21:35:52 -04:00
Team 2648	cca7d68766	before auto manipultor drift fixds	2025-04-17 16:01:39 -04:00
Team 2648	a8a597985f	good 2 piece	2025-04-16 17:46:16 -04:00
Team 2648	060b39669f	attempted auto changes	2025-04-16 15:22:24 -04:00
Tylr-J42	4ada896603	pre champs changes	2025-04-15 02:14:49 -04:00
Tylr-J42	dd26ff6de4	faster 3 piece	2025-04-13 05:29:38 -04:00
Tylr-J42	0589463c4e	pose, logger, and barge shot tweaks	2025-04-13 04:32:46 -04:00
Team 2648	339bf642a1	post-dcmp code	2025-04-08 17:30:11 -04:00
Team 2648	c75554dfc5	before selectino	2025-04-05 08:23:29 -04:00
Team 2648	e98b3a585e	2 piece worked in match	2025-04-04 11:09:09 -04:00
Team 2648	83db16794f	changes to vision filtering and more logging	2025-04-04 09:22:33 -04:00
Team 2648	3dcbac25cc	auto sucky	2025-04-03 19:59:58 -04:00
Team 2648	a391cc7910	algae setpoints	2025-04-03 08:20:37 -04:00
Team 2648	a19285cb0b	tweaks to L1 and auto align good when PIs are on	2025-03-28 23:33:24 -04:00
Team 2648	f6c2a82779	saved encoder offset good	2025-03-28 17:49:40 -04:00
Tylr-J42	dead4a7289	correcting import error from old subsystem	2025-03-28 01:27:28 -04:00
Tylr-J42	9b7d2b45a4	integrated indexer motor with manipulator	2025-03-28 01:21:28 -04:00
Team 2648	11512e140c	added the indexer	2025-03-27 19:04:10 -04:00
NoahLacks63	c7071d409b	added a faster auto shoot and INDENTATION	2025-03-25 00:51:36 +00:00
Tylr-J42	23e2ad5a9b	improving auto align command implementation	2025-03-24 02:13:02 -04:00
Tylr-J42	d693faf5c9	mirrored paths and vision disconnection detection	2025-03-24 01:00:35 -04:00
Team 2648	c9f6928806	progress on 3 piece auto	2025-03-22 20:47:47 -04:00
Team 2648	fdf837ab10	working 2.5 piece	2025-03-22 19:23:19 -04:00
Team 2648	be961d286b	2 piece left works	2025-03-22 19:19:09 -04:00