diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index ca821966..b3c88a81 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -147,7 +147,6 @@ public void setupPathPlanner() public double getDistanceToSpeaker() { int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - allianceAprilTag = 7; // Taken from PhotonUtils.getDistanceToPose Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); return getPose().getTranslation().getDistance(speakerAprilTagPose.toPose2d().getTranslation()); @@ -161,8 +160,6 @@ public double getDistanceToSpeaker() public Rotation2d getSpeakerYaw() { int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; - allianceAprilTag = 7; - // Taken from PhotonUtils.getYawToPose() Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); Translation2d relativeTrl = speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation();