Package frc.robot.shared.bindings

Class TrenchZoneConfig

java.lang.Object
frc.robot.shared.config.AbstractConfig
frc.robot.shared.bindings.TrenchZoneConfig
public class TrenchZoneConfig extends AbstractConfig
Defines a rectangular trench zone on the field and provides geometric detection for whether a straight-line path crosses it. When a d-pad pathfinding route intersects a trench zone, intermediate waypoints are inserted at the zone entry and exit so the robot passes through with the correct heading.

All coordinates are in field (blue-alliance) frame. Each hub has two trenches (north and south), giving four zones total. The X boundaries cover the hub width with a buffer, and the Y boundaries split at the hub center so the correct trench center Y is used for waypoints. Each zone's trenchCenterYMeters is derived from the AprilTags at the trench opening (e.g., tags 22/23 for the blue north trench at y=7.411). The heading rule is purely directional: 180 degrees for +X travel, 0 degrees for -X travel.

  • Field Details

    • enabled

      public boolean enabled
      Whether this trench zone is active.

      Disabled zones are skipped during intersection checks.

    • minXMeters

      public double minXMeters
      Minimum X-coordinate of the trench zone in meters (field coordinates).

    • maxXMeters

      public double maxXMeters
      Maximum X-coordinate of the trench zone in meters (field coordinates).

    • minYMeters

      public double minYMeters
      Minimum Y-coordinate of the trench zone in meters (field coordinates).

    • maxYMeters

      public double maxYMeters
      Maximum Y-coordinate of the trench zone in meters (field coordinates).

    • trenchCenterYMeters

      public double trenchCenterYMeters
      Y-coordinate of the trench center in meters (field coordinates), derived from the AprilTags centered on the trench opening.

      Both entry and exit waypoints use this Y value so the robot lines up with the center of the trench passage.

    • bufferMeters

      public double bufferMeters
      Buffer distance in meters added outside the zone boundary when computing entry and exit waypoints.

      The entry waypoint is placed this far outside the zone edge so the robot finishes its heading correction before physically entering the trench. A value of 0.5 meters gives roughly half a robot length of margin.

  • Constructor Details

    • TrenchZoneConfig

      public TrenchZoneConfig()

  • Method Details

    • getEnabled

      public boolean getEnabled()
      Reads whether this trench zone is enabled.
      Returns:
      true if the zone should be checked during intersection tests

    • getMinXMeters

      public double getMinXMeters()
      Reads the tunable minimum X-coordinate of the zone in meters.
      Returns:
      minimum X boundary in meters

    • getMaxXMeters

      public double getMaxXMeters()
      Reads the tunable maximum X-coordinate of the zone in meters.
      Returns:
      maximum X boundary in meters

    • getMinYMeters

      public double getMinYMeters()
      Reads the tunable minimum Y-coordinate of the zone in meters.
      Returns:
      minimum Y boundary in meters

    • getMaxYMeters

      public double getMaxYMeters()
      Reads the tunable maximum Y-coordinate of the zone in meters.
      Returns:
      maximum Y boundary in meters

    • getTrenchCenterYMeters

      public double getTrenchCenterYMeters()
      Reads the tunable trench center Y-coordinate in meters.
      Returns:
      trench center Y in meters

    • getBufferMeters

      public double getBufferMeters()
      Reads the tunable buffer distance in meters.
      Returns:
      buffer distance outside the zone edge in meters

    • intersectsLineSegment

      public boolean intersectsLineSegment(edu.wpi.first.math.geometry.Pose2d start, edu.wpi.first.math.geometry.Pose2d target)
      Tests whether the straight-line segment from one pose to another intersects this trench zone rectangle.

      Uses the Cohen-Sutherland line-clipping algorithm to test a line segment against the axis-aligned bounding box. This is a conservative heuristic: false positives (the line crosses the zone but the PathPlanner AD* pathfinder routes around) cause a harmless detour through the entry/exit waypoints, while false negatives are dangerous. Size the zones generously to minimize false negatives.

      Parameters:
      start - starting pose in field coordinates (typically the robot's current odometry pose)
      target - target pose in field coordinates (already alliance-flipped)
      Returns:
      true if the line segment from start to target crosses this zone

    • computeEntryWaypoint

      public edu.wpi.first.math.geometry.Pose2d computeEntryWaypoint(edu.wpi.first.math.geometry.Pose2d start, edu.wpi.first.math.geometry.Pose2d target)
      Computes the entry waypoint just outside the trench zone on the side closest to the starting position.

      The waypoint is placed at the zone's X-edge (plus buffer) on the start side, at the configured trench center Y derived from the AprilTags centered on the trench opening. The heading is set to the required trench heading based on travel direction: 180 degrees for +X travel, 0 degrees for -X travel.

      Parameters:
      start - starting pose in field coordinates
      target - target pose in field coordinates
      Returns:
      entry waypoint pose with the required heading for trench traversal

    • computeExitWaypoint

      public edu.wpi.first.math.geometry.Pose2d computeExitWaypoint(edu.wpi.first.math.geometry.Pose2d start, edu.wpi.first.math.geometry.Pose2d target)
      Computes the exit waypoint just outside the trench zone on the side closest to the target position.

      The waypoint is placed at the zone's X-edge (plus buffer) on the target side, at the configured trench center Y so the robot exits aligned with the trench passage. The heading matches the entry heading so the robot maintains a consistent orientation through the entire trench transit.

      Parameters:
      start - starting pose in field coordinates
      target - target pose in field coordinates
      Returns:
      exit waypoint pose with the required heading for trench traversal