Expose waypoint diagnostics and clean ranking defaults

Author: Superbro525Alt

src/main/java/org/curtinfrc/frc2026/util/Repulsor/FieldPlanner/FieldPlanner.java

@@ -41,6 +41,7 @@
 import org.curtinfrc.frc2026.util.Repulsor.FieldPlanner.Helpers.FieldPlannerGeometry;
 import org.curtinfrc.frc2026.util.Repulsor.FieldPlanner.Helpers.FieldPlannerGoalManager;
 import org.curtinfrc.frc2026.util.Repulsor.FieldPlanner.Helpers.FieldPlannerWaypointConfig;
+import org.curtinfrc.frc2026.util.Repulsor.FieldPlanner.Helpers.FieldPlannerWaypointStatus;
 import org.curtinfrc.frc2026.util.Repulsor.FieldPlanner.Helpers.FieldPlannerWaypointStrategy;
 import org.curtinfrc.frc2026.util.Repulsor.FieldPlanner.Obstacles.GatedAttractorObstacle;
 import org.curtinfrc.frc2026.util.Repulsor.Fields.FieldGeometry;
@@ -376,6 +377,10 @@ public FieldPlannerWaypointConfig getWaypointConfig() {
     return goalManager.getWaypointConfig();
   }
 
+  public FieldPlannerWaypointStatus getWaypointStatus() {
+    return goalManager.getWaypointStatus();
+  }
+
   /**
    * Returns the get requested goal pose value maintained by this Repulsor component.
    *

src/main/java/org/curtinfrc/frc2026/util/Repulsor/FieldPlanner/Helpers/FieldPlannerGoalManager.java

@@ -96,6 +96,8 @@ public final class FieldPlannerGoalManager {
   private boolean stagedCenterReturn = false;
   private boolean stagedExitPhase = false;
   private Translation2d stagedExitPoint = null;
+  private FieldPlannerWaypointDecision lastStrategyDecision =
+      FieldPlannerWaypointDecision.useDefault();
 
   private final List<GatedAttractorObstacle> gatedAttractors;
   private final double fieldLengthMeters;
@@ -219,6 +221,22 @@ public FieldPlannerWaypointConfig getWaypointConfig() {
     return waypointConfig;
   }
 
+  public FieldPlannerWaypointStatus getWaypointStatus() {
+    return new FieldPlannerWaypointStatus(
+        requestedGoal,
+        goal,
+        lastStrategyDecision,
+        stagedAttractor != null,
+        stagedAttractor,
+        stagedExitPoint,
+        stagedGate == null ? null : stagedGate.center,
+        stagedExitPhase,
+        stagedComplete,
+        stagedUsingBypass,
+        stagedCenterReturn,
+        stagedModeTicks);
+  }
+
   /**
    * Updates set requested goal state or telemetry as part of the Repulsor runtime loop. This may
    * mutate local state, NetworkTables output, planner caches, or command-side runtime state
@@ -281,6 +299,7 @@ public boolean updateStagedGoal(Translation2d curPos, List<? extends Obstacle> o
             stagedExitPhase);
     FieldPlannerWaypointDecision decision = waypointStrategy.decide(context);
     if (decision == null) decision = FieldPlannerWaypointDecision.useDefault();
+    lastStrategyDecision = decision;
     if (decision.goesDirectlyToRequestedGoal()) {
       clearStagedState(false);
       goal = requestedGoal;

src/main/java/org/curtinfrc/frc2026/util/Repulsor/FieldPlanner/Helpers/FieldPlannerWaypointStatus.java

@@ -0,0 +1,24 @@
+package org.curtinfrc.frc2026.util.Repulsor.FieldPlanner.Helpers;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+/** Structured snapshot of the goal manager's waypoint/staging state for tests and telemetry. */
+public record FieldPlannerWaypointStatus(
+    Pose2d requestedGoal,
+    Pose2d activeGoal,
+    FieldPlannerWaypointDecision lastStrategyDecision,
+    boolean activeStage,
+    Translation2d stagedAttractor,
+    Translation2d stagedExitPoint,
+    Translation2d stagedGateCenter,
+    boolean exitPhase,
+    boolean stagedComplete,
+    boolean usingBypass,
+    boolean centerReturn,
+    int stagedModeTicks) {
+  public FieldPlannerWaypointStatus {
+    if (lastStrategyDecision == null)
+      lastStrategyDecision = FieldPlannerWaypointDecision.useDefault();
+  }
+}

src/main/java/org/curtinfrc/frc2026/util/Repulsor/Predictive/Model/PredictiveRankingConfig.java

@@ -1,7 +1,5 @@
 package org.curtinfrc.frc2026.util.Repulsor.Predictive.Model;
 
-import org.curtinfrc.frc2026.util.Repulsor.Predictive.PredictiveFieldStateOps;
-
 /** Tunable weights used when ranking field-object setpoints. */
 public record PredictiveRankingConfig(
     double advantageGain,
@@ -12,28 +10,37 @@ public record PredictiveRankingConfig(
     double headingGain,
     double hysteresisBonus,
     double hysteresisPersistSeconds) {
+  public static final double DEFAULT_ADVANTAGE_GAIN = 1.1;
+  public static final double DEFAULT_DISTANCE_COST = 0.10;
+  public static final double DEFAULT_PRESSURE_COST = 0.72;
+  public static final double DEFAULT_CONGESTION_COST = 0.95;
+  public static final double DEFAULT_CAPACITY_GAIN = 0.45;
+  public static final double DEFAULT_HEADING_GAIN = 0.18;
+  public static final double DEFAULT_HYSTERESIS_BONUS = 0.22;
+  public static final double DEFAULT_HYSTERESIS_PERSIST_SECONDS = 0.8;
+
   public static PredictiveRankingConfig defaults() {
     return new PredictiveRankingConfig(
-        PredictiveFieldStateOps.ADV_GAIN,
-        PredictiveFieldStateOps.DIST_COST,
-        PredictiveFieldStateOps.PRESSURE_GAIN,
-        PredictiveFieldStateOps.CONGEST_COST,
-        PredictiveFieldStateOps.CAPACITY_GAIN,
-        PredictiveFieldStateOps.HEADING_GAIN,
-        PredictiveFieldStateOps.HYST_BONUS,
-        PredictiveFieldStateOps.HYST_PERSIST_S);
+        DEFAULT_ADVANTAGE_GAIN,
+        DEFAULT_DISTANCE_COST,
+        DEFAULT_PRESSURE_COST,
+        DEFAULT_CONGESTION_COST,
+        DEFAULT_CAPACITY_GAIN,
+        DEFAULT_HEADING_GAIN,
+        DEFAULT_HYSTERESIS_BONUS,
+        DEFAULT_HYSTERESIS_PERSIST_SECONDS);
   }
 
   public PredictiveRankingConfig {
-    advantageGain = finiteNonNegative(advantageGain, PredictiveFieldStateOps.ADV_GAIN);
-    distanceCost = finiteNonNegative(distanceCost, PredictiveFieldStateOps.DIST_COST);
-    pressureCost = finiteNonNegative(pressureCost, PredictiveFieldStateOps.PRESSURE_GAIN);
-    congestionCost = finiteNonNegative(congestionCost, PredictiveFieldStateOps.CONGEST_COST);
-    capacityGain = finiteNonNegative(capacityGain, PredictiveFieldStateOps.CAPACITY_GAIN);
-    headingGain = finiteNonNegative(headingGain, PredictiveFieldStateOps.HEADING_GAIN);
-    hysteresisBonus = finiteNonNegative(hysteresisBonus, PredictiveFieldStateOps.HYST_BONUS);
+    advantageGain = finiteNonNegative(advantageGain, DEFAULT_ADVANTAGE_GAIN);
+    distanceCost = finiteNonNegative(distanceCost, DEFAULT_DISTANCE_COST);
+    pressureCost = finiteNonNegative(pressureCost, DEFAULT_PRESSURE_COST);
+    congestionCost = finiteNonNegative(congestionCost, DEFAULT_CONGESTION_COST);
+    capacityGain = finiteNonNegative(capacityGain, DEFAULT_CAPACITY_GAIN);
+    headingGain = finiteNonNegative(headingGain, DEFAULT_HEADING_GAIN);
+    hysteresisBonus = finiteNonNegative(hysteresisBonus, DEFAULT_HYSTERESIS_BONUS);
     hysteresisPersistSeconds =
-        finiteNonNegative(hysteresisPersistSeconds, PredictiveFieldStateOps.HYST_PERSIST_S);
+        finiteNonNegative(hysteresisPersistSeconds, DEFAULT_HYSTERESIS_PERSIST_SECONDS);
   }
 
   public PredictiveRankingConfig withDistanceCost(double value) {

src/main/java/org/curtinfrc/frc2026/util/Repulsor/Predictive/PredictiveFieldStateOps.java

@@ -244,49 +244,50 @@ public void markCollectDepleted(Translation2d p, double cellM, double strength)
    * Configuration value for hyst persist s. The valid range and tuning source are defined by the
    * owning subsystem or field profile.
    */
-  public static final double HYST_PERSIST_S = 0.8;
+  public static final double HYST_PERSIST_S =
+      PredictiveRankingConfig.DEFAULT_HYSTERESIS_PERSIST_SECONDS;
 
   /**
    * Configuration value for hyst bonus. The valid range and tuning source are defined by the owning
    * subsystem or field profile.
    */
-  public static final double HYST_BONUS = 0.22;
+  public static final double HYST_BONUS = PredictiveRankingConfig.DEFAULT_HYSTERESIS_BONUS;
 
   /**
    * Configuration value for adv gain. The valid range and tuning source are defined by the owning
    * subsystem or field profile.
    */
-  public static final double ADV_GAIN = 1.1;
+  public static final double ADV_GAIN = PredictiveRankingConfig.DEFAULT_ADVANTAGE_GAIN;
 
   /**
    * Configuration value for dist cost. The valid range and tuning source are defined by the owning
    * subsystem or field profile.
    */
-  public static final double DIST_COST = 0.10;
+  public static final double DIST_COST = PredictiveRankingConfig.DEFAULT_DISTANCE_COST;
 
   /**
    * Configuration value for pressure gain. The valid range and tuning source are defined by the
    * owning subsystem or field profile.
    */
-  public static final double PRESSURE_GAIN = 0.72;
+  public static final double PRESSURE_GAIN = PredictiveRankingConfig.DEFAULT_PRESSURE_COST;
 
   /**
    * Configuration value for congest cost. The valid range and tuning source are defined by the
    * owning subsystem or field profile.
    */
-  public static final double CONGEST_COST = 0.95;
+  public static final double CONGEST_COST = PredictiveRankingConfig.DEFAULT_CONGESTION_COST;
 
   /**
    * Configuration value for capacity gain. The valid range and tuning source are defined by the
    * owning subsystem or field profile.
    */
-  public static final double CAPACITY_GAIN = 0.45;
+  public static final double CAPACITY_GAIN = PredictiveRankingConfig.DEFAULT_CAPACITY_GAIN;
 
   /**
    * Configuration value for heading gain. The valid range and tuning source are defined by the
    * owning subsystem or field profile.
    */
-  public static final double HEADING_GAIN = 0.18;
+  public static final double HEADING_GAIN = PredictiveRankingConfig.DEFAULT_HEADING_GAIN;
 
   /**
    * Configuration value for collect value gain. The valid range and tuning source are defined by

src/test/java/org/curtinfrc/frc2026/util/Repulsor/FieldPlanner/Helpers/FieldPlannerGoalManagerTest.java

@@ -386,6 +386,11 @@ void customStrategyCanDefineNonCorridorWaypointingWithoutGates() {
     assertFalse(manager.updateStagedGoal(new Translation2d(2.0, 2.0), List.of()));
     assertEquals(4.0, manager.getGoalTranslation().getX(), EPS);
     assertEquals(6.0, manager.getGoalTranslation().getY(), EPS);
+    FieldPlannerWaypointStatus status = manager.getWaypointStatus();
+    assertEquals(FieldPlannerWaypointDecision.Mode.STAGE, status.lastStrategyDecision().mode());
+    assertTrue(status.activeStage());
+    assertEquals(4.0, status.stagedAttractor().getX(), EPS);
+    assertEquals(requested.getX(), status.requestedGoal().getX(), EPS);
 
     assertFalse(manager.updateStagedGoal(new Translation2d(3.0, 5.0), List.of()));
     assertEquals(4.0, manager.getGoalTranslation().getX(), EPS);