nearest trench tag stuff

[Fatblabs]

No description provided.

[gemini-code-assist]

Code Review

This pull request introduces TrenchUtil, a utility class for calculating and retrieving the midpoints of trench AprilTag pairs, and updates LaunchCalculator to utilize it. Review feedback identifies a logic error in isUnderClimb where trench tags were incorrectly substituted for under-climb tags. Additionally, concerns were raised regarding potential startup crashes if AprilTags are missing from the field layout during static initialization, and the risk of false positives in proximity checks when coordinates fall outside field boundaries.

[gemini-code-assist]

The isUnderClimb method incorrectly uses TrenchUtil.nearestTrenchTag, which is designed to return the location of trench tags. This method should instead determine the nearest tag from the underclimbTags list (tags 15 and 31). Using trench tag midpoints here will result in incorrect proximity detection for the climb structure, potentially failing to protect the hood when the robot is actually under it.

Suggested change

	Pose2d nearestTag = TrenchUtil.nearestTrenchTag(turretPose.getTranslation());
	Pose2d nearestTag = turretPose.nearest(underclimbTags);

[gemini-code-assist]

Using orElseThrow with IllegalStateException in a static initializer will cause the TrenchUtil class to fail to load if the specified AprilTag is missing from the field layout. This could lead to a robot code crash at startup. It is safer to use System.out.println to log the issue to the terminal and provide a fallback pose to allow the robot to continue operating, as terminal output is preferred for debugging in this project.

References

1. Use System.out.println for debugging output that is intended for the terminal, as opposed to logging frameworks that might target the driver station.

[gemini-code-assist]

Returning Pose2d.kZero when the translation is outside field bounds can lead to false positives in proximity checks (e.g., if the robot is near the origin, dx and dy will be small). Since the quadrant-based logic for redSide and highY works for any coordinates, it is safer to remove this bounds check and return the closest trench midpoint regardless of whether the pose is strictly within the field boundaries.

    boolean redSide = translation.getX() >= FIELD_LENGTH / 2.0;
    boolean highY = translation.getY() >= FIELD_WIDTH / 2.0;