fix: working flipping on counter shuffle

flamingchickens1540 · Apr 18, 2024 · 83e90ad · 83e90ad
1 parent 5430a26
commit 83e90ad
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 3 deletions.
diff --git 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
@@ -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<Pose2d> 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
@@ -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;
     }
 }