Skip to content

nearest trench tag stuff #255

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Fatblabs wants to merge 1 commit into
base: main
Choose a base branch
from
+68 −2
Open

nearest trench tag stuff #255

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
import frc.robot.util.AllianceUtils;
import frc.robot.util.GetTargetFromPose;
import frc.robot.util.TrenchUtil;
import frc.robot.util.tuning.LauncherConstants;
import java.util.ArrayList;
import java.util.List;
Expand Down Expand Up @@ -430,14 +431,14 @@ public static boolean isApproachingTrench(Pose2d robotPose, ChassisSpeeds speeds
}

public static boolean isCloseToTrench(Pose2d pose) {
Pose2d nearestTag = pose.nearest(trenchTags);
Pose2d nearestTag = TrenchUtil.nearestTrenchTag(pose.getTranslation());
double dx = Math.abs(pose.getX() - nearestTag.getX());
double dy = Math.abs(pose.getY() - nearestTag.getY());
return dx < TURRET_TO_TRENCH_TOLERANCE_X && dy < TURRET_TO_TRENCH_TOLERANCE_Y;
}

public static boolean isUnderClimb(Pose2d turretPose) {
Pose2d nearestTag = turretPose.nearest(underclimbTags);
Pose2d nearestTag = TrenchUtil.nearestTrenchTag(turretPose.getTranslation());

@gemini-code-assist gemini-code-assist Bot Apr 29, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

high

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);

double dx = Math.abs(turretPose.getX() - nearestTag.getX());
double dy = Math.abs(turretPose.getY() - nearestTag.getY());
return dx < TURRET_TO_UNDERCLIMB_TOLERANCE_X && dy < TURRET_TO_UNDERCLIMB_TOLERANCE_Y;
Expand Down
65 changes: 65 additions & 0 deletions src/main/java/frc/robot/util/TrenchUtil.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,65 @@
package frc.robot.util;

import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rectangle2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.Objects;

public final class TrenchUtil {
private static final double FIELD_LENGTH = AllianceUtils.FIELD_LAYOUT.getFieldLength();
private static final double FIELD_WIDTH = AllianceUtils.FIELD_LAYOUT.getFieldWidth();
private static final Rectangle2d FIELD_BOUNDS =
new Rectangle2d(new Translation2d(0.0, 0.0), new Translation2d(FIELD_LENGTH, FIELD_WIDTH));

private static final Pose2d BLUE_LOW_Y_TRENCH = midpointBetweenTags(17, 28);
private static final Pose2d BLUE_HIGH_Y_TRENCH = midpointBetweenTags(22, 23);
private static final Pose2d RED_LOW_Y_TRENCH = midpointBetweenTags(6, 7);
private static final Pose2d RED_HIGH_Y_TRENCH = midpointBetweenTags(1, 12);

private TrenchUtil() {}

private static Pose2d midpointBetweenTags(int tagA, int tagB) {
AprilTagFieldLayout fieldLayout = AllianceUtils.FIELD_LAYOUT;
Translation2d tagATranslation =
fieldLayout
.getTagPose(tagA)
.orElseThrow(() -> new IllegalStateException("AprilTag " + tagA + " is not present"))

@gemini-code-assist gemini-code-assist Bot Apr 29, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

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.

.getTranslation()
.toTranslation2d();
Translation2d tagBTranslation =
fieldLayout
.getTagPose(tagB)
.orElseThrow(() -> new IllegalStateException("AprilTag " + tagB + " is not present"))
.getTranslation()
.toTranslation2d();

return new Pose2d(tagATranslation.interpolate(tagBTranslation, 0.5), Rotation2d.kZero);
}

/**
* Returns the midpoint of the trench AprilTag pair in the same WPILib blue-origin field
* coordinate frame used by AprilTagFieldLayout. X spans the field length from blue to red, and Y
* spans the field width from the blue alliance's right to left.
*/
public static Pose2d nearestTrenchTag(Translation2d translation) {
Objects.requireNonNull(translation, "translation");

if (!FIELD_BOUNDS.contains(translation)) {
DriverStation.reportWarning(
"Translation is outside the WPILib field bounds: " + translation, false);
return Pose2d.kZero;
}

boolean redSide = translation.getX() >= FIELD_LENGTH / 2.0;
boolean highY = translation.getY() >= FIELD_WIDTH / 2.0;
Comment on lines +50 to +57

@gemini-code-assist gemini-code-assist Bot Apr 29, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

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;


if (redSide) {
return highY ? RED_HIGH_Y_TRENCH : RED_LOW_Y_TRENCH;
}

return highY ? BLUE_HIGH_Y_TRENCH : BLUE_LOW_Y_TRENCH;
}
}
Loading