From c9fdf8de42cf8f8363bd0998757270068492c94b Mon Sep 17 00:00:00 2001
From: iefomit <timofei.stem@gmail.com>
Date: Tue, 24 Feb 2026 18:29:50 -0800
Subject: [PATCH] bug fixes

---
 .../robot/commands/drive_comm/DefaultDriveCommand.java   | 4 ++--
 .../frc/robot/controls/PS5ControllerDriverConfig.java    | 9 ---------
 .../java/frc/robot/util/TrenchAssist/TrenchAssist.java   | 2 +-
 3 files changed, 3 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
index c481e0d..daa9b1e 100644
--- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
@@ -12,7 +12,7 @@ import frc.robot.Robot;
 import frc.robot.constants.swerve.DriveConstants;
 import frc.robot.controls.BaseDriverConfig;
 import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.util.TrenchAssist.TrenchAssist2;
+import frc.robot.util.TrenchAssist.TrenchAssist;
 import frc.robot.util.TrenchAssist.TrenchAssistConstants;
 import frc.robot.util.Vision.DriverAssist;
 
@@ -88,7 +88,7 @@ public class DefaultDriveCommand extends Command {
 
         Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist());
         if (swerve.getTrenchAssist()) {
-            drive(TrenchAssist2.calculate(swerve, corrected, trenchAssistPid));
+            drive(TrenchAssist.calculate(swerve, corrected, trenchAssistPid));
         } else {
             trenchAssistPid.reset();
             drive(corrected);
diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 89337f1..b646ac8 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -80,15 +80,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                 () -> false, getDrivetrain()).withTimeout(2));
 
         // Trench align
-        controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
-            getDrivetrain().setTrenchAssist(true);
-            getDrivetrain().setTrenchAlign(true);
-        }))
-                .onFalse(new InstantCommand(() -> {
-                    getDrivetrain().setTrenchAssist(false);
-                    getDrivetrain().setTrenchAlign(false);
-                }));
-
         controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
             getDrivetrain().setTrenchAssist(true);
             getDrivetrain().setTrenchAlign(true);
diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
index 47a29ed..4d2a3e3 100644
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
@@ -32,7 +32,7 @@ public class TrenchAssist {
 
         double correctionVelocity = pid.calculate(distanceFromSlideLatitude, 0);
 
-        if (distanceFromSlideLatitude < Units.inchesToMeters(3)) {
+        if (Math.abs(distanceFromSlideLatitude) < Units.inchesToMeters(3)) {
             correctionVelocity = 0.0;
         }
 
-- 
2.39.5