made speaker poses 3D so less code is required

src/main/java/frc/robot/Constants.java

@@ -4,13 +4,11 @@
 package frc.robot;
 
 import com.pathplanner.lib.util.PIDConstants;
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import frc.robot.utils.AllianceFieldConstants;
-import frc.robot.utils.UtilFuncs;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -133,12 +131,12 @@ public static class Ports {
 
   // static field constants
   public static class FieldConstants {
-    public static final double SPEAKER_HEIGHT = .200;
+    public static final double SPEAKER_HEIGHT = 2;
 
     public static final int SPEAKER_TAG_BLUE = 7;
     public static final int SPEAKER_TAG_RED = 4;
 
-    public static final Pose2d SPEAKER_POSE_BLUE = new Pose2d(0.25, 5.5, Rotation2d.fromDegrees(180.0));
-    public static final Pose2d SPEAKER_POSE_RED = new Pose2d(16.3, 5.5, Rotation2d.fromDegrees(0.0));
+    public static final Pose3d SPEAKER_POSE_BLUE = new Pose3d(0.25, 5.5, SPEAKER_HEIGHT, new Rotation3d(0, 0, 180));
+    public static final Pose3d SPEAKER_POSE_RED = new Pose3d(16.3, 5.5, SPEAKER_HEIGHT, new Rotation3d(0, 0, 0));
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -8,7 +8,6 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -17,9 +16,7 @@
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
-import frc.robot.commands.elevator.HoldElevator;
 import frc.robot.commands.shooter.AutoAim;
-import frc.robot.commands.shooter.HoldShooter;
 import frc.robot.commands.shooter.SpinShooter;
 import frc.robot.commands.swerve.BrakeSwerve;
 import frc.robot.commands.swerve.ResetPose;
@@ -29,7 +26,6 @@
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
-import frc.robot.utils.UtilFuncs;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -26,12 +26,10 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
-import frc.robot.Constants.FieldConstants;
 import frc.robot.utils.BNO055;
 import frc.robot.utils.SwerveModule;
 import frc.robot.utils.UtilFuncs;
@@ -408,17 +406,16 @@ public double[] speakerAngles() {
     double xSpeakerAngle;
     double ySpeakerAngle;
 
-    int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG;
-    Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get();
+    Pose3d speakerPose = Constants.FIELD_CONSTANTS.SPEAKER_POSE;
 
-    Translation2d tagTranslation = new Translation2d(tagPose.getX(), tagPose.getY());
+    Translation2d speakerTranslation = new Translation2d(speakerPose.getX(), speakerPose.getY());
     Translation2d botTranslation = getPose().getTranslation();
 
-    Translation2d distanceVec = tagTranslation.minus(botTranslation);
+    Translation2d distanceVec = speakerTranslation.minus(botTranslation);
 
     xSpeakerAngle = MathUtil.inputModulus(distanceVec.getAngle().getDegrees(), -180, 180);
 
-    double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED; // TODO: move to Constants?
+    double zDifference = speakerPose.getZ() - Constants.Physical.SHOOTER_HEIGHT_STOWED; // TODO: move to Constants?
     ySpeakerAngle = Math.toDegrees(Math.atan(zDifference / distanceVec.getNorm()));
 
     double[] angles = {

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -9,7 +9,6 @@
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
 import frc.robot.utils.LimelightHelper;
 
 import java.util.Optional;
@@ -122,13 +121,4 @@ public double[] tagAngleOffsets(int ID) {
 
     return angles;
   }
-
-  /**
-   * The tag angle offsets to the tag on the speaker (automatically knows which tag to check).
-   * 
-   * @return [tx, ty] or null.
-   */
-  public double[] anglesToSpeaker() {
-    return tagAngleOffsets(Constants.FIELD_CONSTANTS.SPEAKER_TAG);
-  }
 }

src/main/java/frc/robot/utils/AllianceFieldConstants.java

@@ -3,7 +3,7 @@
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
-import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.Constants;
 
@@ -15,11 +15,8 @@ public class AllianceFieldConstants {
      */
     public final AprilTagFieldLayout APRILTAG_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
 
-    /** The speaker's tag ID. */
-    public final int SPEAKER_TAG;
-
-    /** The speaker's 2d pose. */
-    public final Pose2d SPEAKER_POSE;
+    /** The speaker's 3d pose. */
+    public final Pose3d SPEAKER_POSE;
 
     /** Creates a new AllianceFieldConstants. */
     public AllianceFieldConstants() {
@@ -28,10 +25,8 @@ public AllianceFieldConstants() {
         Alliance alliance = UtilFuncs.GetCurrentAlliance();
 
         if (alliance == Alliance.Blue) {
-            SPEAKER_TAG = Constants.FieldConstants.SPEAKER_TAG_BLUE;
             SPEAKER_POSE = Constants.FieldConstants.SPEAKER_POSE_BLUE;
         } else {
-            SPEAKER_TAG = Constants.FieldConstants.SPEAKER_TAG_RED;
             SPEAKER_POSE = Constants.FieldConstants.SPEAKER_POSE_RED;
         }
     }