Log Game State

src/main/java/frc/game/Field.java

@@ -0,0 +1,105 @@
+package frc.game;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public class Field {
+  public static final double FIELD_WIDTH = 317.7;
+  public static final double ALLIANCE_ZONE_LENGTH = 158.6;
+  public static final double NEUTRAL_ZONE_LENGTH = 283;
+
+  enum Region {
+    BlueZone(build().xpos(0).ypos(0).xlen(ALLIANCE_ZONE_LENGTH).ylen(FIELD_WIDTH)),
+    NeutralZone(
+        build().xpos(ALLIANCE_ZONE_LENGTH).ypos(0).xlen(NEUTRAL_ZONE_LENGTH).ylen(FIELD_WIDTH)),
+    RedZone(
+        build()
+            .xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH)
+            .ypos(0)
+            .xlen(ALLIANCE_ZONE_LENGTH)
+            .ylen(FIELD_WIDTH)),
+    RedLeftTrench(
+        build()
+            .xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH)
+            .ypos(FIELD_WIDTH)
+            .xlen(65.65)
+            .ylen(47)),
+    RedRightTrench(
+        build().xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH).ypos(0).xlen(65.65).ylen(47)),
+    BlueLeftTrench(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(FIELD_WIDTH).xlen(65.65).ylen(47)),
+    BlueRightTrench(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(0).xlen(65.65).ylen(47)),
+    BlueDepot(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(65.65).xlen(42).ylen(27)),
+    RedDepot(build().xpos(FIELD_WIDTH).ypos(91.4 + 73).xlen(42).ylen(27)),
+    BlueTower(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(91.4).xlen(49.25).ylen(45)),
+    RedTower(build().xpos(FIELD_WIDTH).ypos(91.4).xlen(49.25).ylen(45)),
+    BlueHub(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(91.4).xlen(47).ylen(47)),
+    RedHub(build().xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH).ypos(91.4).xlen(47).ylen(47)),
+    RedLeftBump(
+        build().xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH).ypos(135.8).xlen(73).ylen(44.4)),
+    RedRightBump(
+        build().xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH).ypos(47).xlen(73).ylen(44.4)),
+    BlueLeftBump(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(135.8).xlen(73).ylen(44.4)),
+    BlueRightBump(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(47).xlen(73).ylen(44.4)),
+    FuelArrangement(
+        build().xpos(NEUTRAL_ZONE_LENGTH / 2 + 72 / 2).ypos(317.7 - 47 * 2).xlen(72).ylen(206));
+
+    public final double xpos;
+    public final double ypos;
+    public final double xlen;
+    public final double ylen;
+
+    private Region(Builder builder) {
+      this.xpos = builder.xpos;
+      this.ypos = builder.ypos;
+      this.xlen = builder.xlen;
+      this.ylen = builder.ylen;
+    }
+
+    public static Builder build() {
+      return new Builder();
+    }
+
+    public static class Builder {
+      double xpos;
+      double ypos;
+      double xlen;
+      double ylen;
+
+      public Builder xpos(double xpos) {
+        this.xpos = xpos;
+        return this;
+      }
+
+      public Builder ypos(double ypos) {
+        this.ypos = ypos;
+        return this;
+      }
+
+      public Builder xlen(double xlen) {
+        this.xlen = xlen;
+        return this;
+      }
+
+      public Builder ylen(double ylen) {
+        this.ylen = ylen;
+        return this;
+      }
+    }
+
+    public boolean contains(Pose2d pose) {
+      return (pose.getX() >= xpos)
+          && (pose.getX() < xpos + xlen)
+          && (pose.getY() >= ypos)
+          && (pose.getY() < ypos + ylen);
+    }
+  }
+
+  public static Region getZone(Pose2d pose) {
+    if (pose.getX() < Region.NeutralZone.xpos) {
+      return Region.BlueZone;
+    }
+    if (pose.getX() < Region.RedZone.xpos) {
+      return Region.NeutralZone;
+    }
+    return Region.RedZone;
+  }
+}

src/main/java/frc/game/GameState.java

@@ -0,0 +1,121 @@
+package frc.game;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import java.util.List;
+import java.util.Optional;
+import org.littletonrobotics.junction.Logger;
+
+public class GameState {
+
+  enum GamePhase {
+    None("0:00 - 0:00"),
+    Autonomous("0:20 - 0:00"),
+    Transition("2:20 - 2:10"),
+    Shift1("2:10 - 1:45"),
+    Shift2("1:45 - 1:20"),
+    Shift3("1:20 - 0:55"),
+    Shift4("0:55 - 0:30"),
+    EndGame("0:30 - 0:00");
+
+    public static final List<GamePhase> TELEOP =
+        List.of(Transition, Shift1, Shift2, Shift3, Shift4, EndGame);
+
+    final double countDownFrom;
+    final double countDownUntil;
+
+    private GamePhase(String timer) {
+      var times = timer.split("-");
+      this.countDownFrom = parseSeconds(times[0]);
+      this.countDownUntil = parseSeconds(times[1]);
+    }
+
+    private static int parseSeconds(String time) {
+      var parts = time.trim().split(":");
+      return Integer.parseInt(parts[0]) * 60 + Integer.parseInt(parts[1]);
+    }
+  }
+
+  private static Alliance autoWinner;
+  private static Alliance myAlliance;
+
+  public static GamePhase getCurrentPhase() {
+    if (!DriverStation.isDSAttached() && !DriverStation.isFMSAttached()) {
+      return GamePhase.None;
+    }
+    if (DriverStation.isAutonomous()) {
+      return GamePhase.Autonomous;
+    }
+    // Must be in match and teleop
+    var t = getMatchTime();
+    for (var gamePhase : GamePhase.TELEOP) {
+      if (t <= gamePhase.countDownFrom && t > gamePhase.countDownUntil) {
+        return gamePhase;
+      }
+    }
+    return GamePhase.None;
+  }
+
+  public static Optional<Alliance> getMyAlliance() {
+    if (myAlliance == null) {
+      myAlliance = DriverStation.getAlliance().orElse(null);
+    }
+    return Optional.ofNullable(myAlliance);
+  }
+
+  public static Optional<Alliance> getAutoWinner() {
+    if (autoWinner == null) {
+      var gameData = DriverStation.getGameSpecificMessage();
+      if (gameData.length() > 0) {
+        autoWinner =
+            switch (gameData.charAt(0)) {
+              case 'B' -> Alliance.Blue;
+              case 'R' -> Alliance.Red;
+              default -> null;
+            };
+      }
+    }
+    return Optional.ofNullable(autoWinner);
+  }
+
+  public static boolean isMyHubActive() {
+    switch (getCurrentPhase()) {
+      case None:
+      case Autonomous:
+      case Transition:
+      case EndGame:
+        return true;
+      case Shift1:
+      case Shift3:
+        return autoWinner != null && myAlliance != null && autoWinner != myAlliance;
+      case Shift2:
+      case Shift4:
+        return autoWinner != null && myAlliance != null && autoWinner == myAlliance;
+      default:
+        return false;
+    }
+  }
+
+  public static double getMatchTime() {
+    return DriverStation.getMatchTime();
+  }
+
+  public static Optional<Alliance> getAlliance() {
+    return DriverStation.getAlliance();
+  }
+
+  public static void logValues() {
+    getMyAlliance();
+    getAutoWinner();
+    Logger.recordOutput("GameState/IsDSAttached", DriverStation.isDSAttached());
+    Logger.recordOutput("GameState/IsFMSAttached", DriverStation.isFMSAttached());
+    Logger.recordOutput("GameState/MatchType", DriverStation.getMatchType());
+    Logger.recordOutput("GameState/IsAutonomus", DriverStation.isAutonomous());
+    Logger.recordOutput("GameState/MatchTime", DriverStation.getMatchTime());
+    Logger.recordOutput("GameState/AutoWinner", autoWinner);
+    Logger.recordOutput("GameState/Alliance", myAlliance);
+    Logger.recordOutput("GameState/GameData", DriverStation.getGameSpecificMessage());
+    Logger.recordOutput("GameState/CurrentPhase", getCurrentPhase());
+    Logger.recordOutput("GameState/IsMyHubActive", isMyHubActive());
+  }
+}

src/main/java/frc/robot/Robot.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.game.GameState;
 import frc.lib.AllianceSelector;
 import frc.lib.AutoOption;
 import frc.lib.AutoSelector;
@@ -184,12 +185,13 @@ public void robotPeriodic() {
     SmartDashboard.putData(CommandScheduler.getInstance());
     SmartDashboard.putData(drive);
 
+    GameState.logValues();
+
     // Return to non-RT thread priority (do not modify the first argument)
     // Threads.setCurrentThreadPriority(false, 10);
   }
 
   /** This function is called once when the robot is disabled. */
-  @Override
   public void disabledInit() {}
 
   /** This function is called periodically when disabled. */