Merge last few fixes from drive practice from Rewrite to main

ThriftyTest/autons/center.txt

@@ -1,6 +1,6 @@
 Align H
 L4
-Align GH & Lower Algae
+Align GH & Upper Algae
 Align Barge Center
 Net
 Upper Algae & Align IJ

ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto

@@ -1 +1,93 @@
-{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align H"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Align GH"}},{"type":"named","data":{"name":"Lower Algae"}}]}},{"type":"named","data":{"name":"Align Barge Center"}},{"type":"named","data":{"name":"Net"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Upper Algae"}},{"type":"named","data":{"name":"Align IJ"}}]}},{"type":"named","data":{"name":"Align Barge Left"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Align LIntake"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false}
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Align H"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "L4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Align GH"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Upper Algae"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Align Barge Center"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Net"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Upper Algae"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Align IJ"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Align Barge Left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Net"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Align LIntake"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": false,
+  "folder": null,
+  "choreoAuto": false
+}

ThriftyTest/src/main/java/frc/robot/generated/TunerConstants.java

@@ -104,7 +104,7 @@ public class TunerConstants {
 
   private static final double kDriveGearRatio = 6.75;
   private static final double kSteerGearRatio = 25;
-  public static final Distance kWheelRadius = Inches.of(2);
+  public static final Distance kWheelRadius = Inches.of(1.935);
 
   private static final boolean kInvertLeftSide = false;
   private static final boolean kInvertRightSide = true;

ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java

@@ -15,7 +15,7 @@ public final class AlgaeConstants {
   protected static final double kHoldVoltage = 2.7;
 
   protected static final double kTorqueCurrentThreshold = 75;
-  protected static final double kSupplyCurrentLimit = 25.0;
+  protected static final double kSupplyCurrentLimit = 40.0;
 
   protected static final double kProcessorScoreTime = 2.0;
   protected static final double kNetScoreTime = 0.4;

ThriftyTest/src/main/java/frc/robot/superstructure/states/Climb.java

@@ -1,6 +1,8 @@
 package frc.robot.superstructure.states;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.pivot.PivotState;
 import frc.robot.superstructure.EnterableState;
 import frc.robot.superstructure.Superstructure.Subsystems;
 
@@ -11,6 +13,8 @@ public class Climb implements EnterableState {
   public Climb() {}
 
   public Command build(Subsystems subsystems) {
-    return subsystems.climber().climb();
+    return Commands.parallel(
+      subsystems.climber().climb(),
+      subsystems.pivot().go(PivotState.Ground));
   }
 }

ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java

@@ -98,7 +98,7 @@ public class LocalizationConstants {
   protected static final double kAvgErr = 0.08;
   protected static final double kErrStdDevs = 0.02;
 
-  public static final boolean kEnableReefFilter = false;
+  public static final boolean kEnableReefFilter = true;
 
   protected static final Set<Integer> kReefIds = Set.of(
       6, 7, 8, 9, 10, 11, // red tags