From 83e90ad6ed57cb2a814fdf04e8c489dc78aa646d Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Thu, 18 Apr 2024 07:04:21 -0500 Subject: [PATCH] fix: working flipping on counter shuffle --- src/main/java/org/team1540/robot2024/Constants.java | 2 +- .../robot2024/commands/shooter/CounterShufflePrepare.java | 5 ++++- .../java/org/team1540/robot2024/util/vision/FlipUtil.java | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index f34abb3..d1e0f0b 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -278,7 +278,7 @@ public static class Targeting { private static final Pose2d SHUFFLE_POSE = new Pose2d(SPEAKER_POSE.getX(), SPEAKER_POSE.getY() + 2, new Rotation2d()); private static final Pose2d COUNTER_SHUFFLE_POSE = - new Pose2d(SPEAKER_POSE.getX() + 8.27, SPEAKER_POSE.getY() + 2, new Rotation2d()); + new Pose2d(SPEAKER_POSE.getX() + 8.27, SPEAKER_POSE.getY(), new Rotation2d()); public static Pose2d getSpeakerPose() { return DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/CounterShufflePrepare.java b/src/main/java/org/team1540/robot2024/commands/shooter/CounterShufflePrepare.java index 909f54f..6f66a47 100644 --- a/src/main/java/org/team1540/robot2024/commands/shooter/CounterShufflePrepare.java +++ b/src/main/java/org/team1540/robot2024/commands/shooter/CounterShufflePrepare.java @@ -6,6 +6,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ProxyCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import org.team1540.robot2024.Constants; import org.team1540.robot2024.subsystems.drive.Drivetrain; @@ -15,6 +17,7 @@ import org.team1540.robot2024.util.vision.FlipUtil; import java.util.function.Supplier; +import java.util.logging.Logger; public class CounterShufflePrepare extends SequentialCommandGroup { public CounterShufflePrepare(Drivetrain drivetrain, Shooter shooter) { @@ -27,7 +30,7 @@ public CounterShufflePrepare(Supplier positionSupplier, Shooter shooter, Rotation2d.fromRadians( Math.atan2(Constants.Targeting.STAGE_MAX_HEIGHT + 1.5 - Constants.Shooter.Pivot.PIVOT_HEIGHT, positionSupplier.get().getTranslation().getDistance( - AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.CLIMB_FAR, DriverStation.Alliance.Blue).toPose2d().getTranslation().plus(new Translation2d(FlipUtil.flipIfRed(-2.277226), 0)) + AprilTagsCrescendo.getInstance().getTag(AprilTagsCrescendo.Tags.CLIMB_FAR).toPose2d().getTranslation().plus(new Translation2d(FlipUtil.flipIfRed(-2.277226), 0)) ) ) ).minus(Constants.Shooter.Pivot.REAL_ZEROED_ANGLE), diff --git a/src/main/java/org/team1540/robot2024/util/vision/FlipUtil.java b/src/main/java/org/team1540/robot2024/util/vision/FlipUtil.java index ea21219..d2ee795 100644 --- a/src/main/java/org/team1540/robot2024/util/vision/FlipUtil.java +++ b/src/main/java/org/team1540/robot2024/util/vision/FlipUtil.java @@ -22,7 +22,7 @@ public static Rotation2d flipIfRed(Rotation2d rotation){ public static double flipIfRed(double num){ return DriverStation.getAlliance().orElse(null) == DriverStation.Alliance.Red - ? num *= 1 + ? num *= -1 : num; } }