From 8ad5a14461819352c3c9536d24e74e084db3b535 Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 7 Apr 2026 09:07:40 -0700 Subject: [PATCH] fix yb --- src/main/java/frc/robot/subsystems/drive/Drive.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 248440a..e941424 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -17,6 +17,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; +import java.util.Set; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.BiFunction; @@ -60,6 +61,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; @@ -361,7 +363,13 @@ public Command resetGyroCmd(Rotation2d rotation) { } public Command resetGyroCmd() { - return resetGyroCmd(DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? Rotation2d.kZero : Rotation2d.fromDegrees(180.0)); + return Commands.defer(() -> { + Rotation2d rotation = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? Rotation2d.kZero + : Rotation2d.fromDegrees(180.0); + return resetGyroCmd(rotation); + }, Set.of(this)); + } public Command stopWithXCmd() {