bug fixes

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 c481e0d87f3d0746c3b9668fa5caaf5d15b1e033..daa9b1eb6f9ab4f4326f88d7dac33a60ef07c8bc 100644 (file)
--- 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 89337f1bbad3014c300367571f5695923dcb8537..b646ac80509844b569765bf82fd6c08cc3e90207 100644 (file)
--- 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 47a29ed0b40434761a3329cc14d0b8dc1fe84d7e..4d2a3e3dc6985dbd706d1bc9e7d9cb9753f677b2 100644 (file)
--- 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;
         }