From edacf725b87ad17d78643cb91195a9751aef3081 Mon Sep 17 00:00:00 2001
From: Junyu Zhu <120147135+junyu101@users.noreply.github.com>
Date: Sat, 29 Mar 2025 16:37:05 -0700
Subject: [PATCH 1/2] Fixed align bug

---
 ...AlignToNearestReefFaceForAlgaeCommand.java | 33 +++++++++++++++++--
 1 file changed, 31 insertions(+), 2 deletions(-)

diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java
index e78e9d35..0cd6a0e5 100644
--- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java
+++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java
@@ -8,6 +8,8 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.units.measure.Angle;
 import xbot.common.command.BaseCommand;
+import xbot.common.math.PIDDefaults;
+import xbot.common.math.PIDManager;
 import xbot.common.math.XYPair;
 import xbot.common.subsystems.drive.control_logic.HeadingModule;
 
@@ -20,6 +22,8 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand {
 
     final HeadingModule headingModule;
 
+    final PIDManager pidManager;
+
     final AprilTagVisionSubsystemExtended vision;
     final DriveSubsystem drive;
     final PoseSubsystem pose;
@@ -31,13 +35,25 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand {
     @Inject
     public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory headingModuleFactory,
                                                  DriveSubsystem drive, PoseSubsystem pose,
-                                                 AprilTagVisionSubsystemExtended vision, OperatorInterface oi) {
+                                                 AprilTagVisionSubsystemExtended vision, OperatorInterface oi,
+                                                 PIDManager.PIDManagerFactory pidManagerFactory) {
         this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());
         this.vision = vision;
         this.drive = drive;
         this.pose = pose;
         this.oi = oi;
         this.addRequirements(drive);
+
+        this.pidManager = pidManagerFactory.create("AlgaePIDManager", new PIDDefaults(
+                1.08, // P
+                0, // I
+                4.0, // D
+                0.0, // F
+                0.6, // Max output
+                -0.6, // Min output
+                0.05, // Error threshold
+                0.005, // Derivative threshold
+                0.2));// Time threshold):
     }
 
     @Override
@@ -60,7 +76,7 @@ public void execute() {
 
         // Everything below here can and will be extracted because it is repeated code with
         // DriveWithSnapToReefTagCommand
-        var centeringTranslation2d = drive.getPowerForRelativePositionChange(new Translation2d(0, robotRelativeTagLocationY));
+        var centeringTranslation2d = getPowerForRelativePositionChange(new Translation2d(0, robotRelativeTagLocationY));
         XYPair centeringVector = new XYPair(centeringTranslation2d.getX(), centeringTranslation2d.getY());
         centeringVector = centeringVector.rotate(pose.getCurrentHeading().getDegrees());
 
@@ -84,4 +100,17 @@ public void execute() {
                 new XYPair()
         );
     }
+
+    public Translation2d getPowerForRelativePositionChange(Translation2d goalPosition) {
+        double goalMagnitude = goalPosition.getNorm();
+
+        if (Math.abs(goalMagnitude) <  0.0001) {
+            goalMagnitude = 0.0001;
+        }
+        Translation2d normalizedGoalVector = goalPosition.div(goalMagnitude);
+
+        double power = -pidManager.calculate(0, goalMagnitude);
+
+        return normalizedGoalVector.times(power);
+    }
 }
\ No newline at end of file

From 0d0553117668868bce47656332479ae81dee7bae Mon Sep 17 00:00:00 2001
From: Junyu Zhu <120147135+junyu101@users.noreply.github.com>
Date: Sat, 29 Mar 2025 16:43:12 -0700
Subject: [PATCH 2/2] added separate pid for align to algae

---
 .../commands/AlignToNearestReefFaceForAlgaeCommand.java     | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java
index 0cd6a0e5..d5b0c898 100644
--- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java
+++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.units.measure.Angle;
 import xbot.common.command.BaseCommand;
+import xbot.common.math.MathUtils;
 import xbot.common.math.PIDDefaults;
 import xbot.common.math.PIDManager;
 import xbot.common.math.XYPair;
@@ -81,7 +82,10 @@ public void execute() {
         centeringVector = centeringVector.rotate(pose.getCurrentHeading().getDegrees());
 
         var fieldVectorTranslation2d = oi.driverGamepad.getLeftFieldOrientedVector();
-        XYPair fieldVectorXYPair = new XYPair(fieldVectorTranslation2d.getX(), fieldVectorTranslation2d.getY());
+        XYPair fieldVectorXYPair = new XYPair(
+                MathUtils.deadband(fieldVectorTranslation2d.getX(), 0.05),
+                MathUtils.deadband(fieldVectorTranslation2d.getY(), 0.05)
+        );
         double railsSimilarityToDriver = fieldVectorXYPair.dotProduct(
                 new XYPair(
                         Math.cos(idealFinalHeading.in(Radians) + Math.PI),