diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java new file mode 100644 index 0000000000..0e48008e88 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDriverStation.java @@ -0,0 +1,148 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.common.dataflow.networktables; + +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.IntegerSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEvent.Kind; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringSubscriber; +import java.util.EnumSet; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +// Helper to print when the robot transitions modes +public class NTDriverStation { + private static final Logger logger = new Logger(NTDriverStation.class, LogGroup.NetworkTables); + + // Copy since stuff was private + public record NtControlWord( + boolean m_enabled, + boolean m_autonomous, + boolean m_test, + boolean m_emergencyStop, + boolean m_fmsAttached, + boolean m_dsAttached) { + public NtControlWord() { + this(false, false, false, false, false, false); + } + } + + private IntegerSubscriber ntControlWord; + + private StringSubscriber eventName; + private IntegerSubscriber matchNumber; + private IntegerSubscriber replayNumber; + private IntegerSubscriber matchType; + private BooleanSubscriber isRedAlliance; + private IntegerSubscriber stationNumber; + + NtControlWord lastControlWord = new NtControlWord(); + + public NTDriverStation(NetworkTableInstance inst) { + NetworkTable fmsTable = inst.getTable("FMSInfo"); + this.ntControlWord = fmsTable.getIntegerTopic("FMSControlData").subscribe(0); + this.eventName = fmsTable.getStringTopic("EventName").subscribe(""); + this.matchType = fmsTable.getIntegerTopic("MatchType").subscribe(0); + this.matchNumber = fmsTable.getIntegerTopic("MatchNumber").subscribe(0); + this.replayNumber = fmsTable.getIntegerTopic("ReplayNumber").subscribe(0); + + this.isRedAlliance = fmsTable.getBooleanTopic("isRedAlliance").subscribe(true); + this.stationNumber = fmsTable.getIntegerTopic("StationNumber").subscribe(0); + + fmsTable.addListener( + "FMSControlData", + EnumSet.of(Kind.kValueAll), + (table, key, event) -> { + if (event.is(Kind.kValueAll) && event.valueData.value.isInteger()) { + // Logger totally isnt thread safe but whatevs + var word = NTDriverStation.getControlWord(event.valueData.value.getInteger()); + + printTransition(this.lastControlWord, word); + printMatchData(); + this.lastControlWord = word; + } + }); + + // Slight data race here but whatever + NtControlWord word = NTDriverStation.getControlWord(this.ntControlWord.get()); + + printTransition(this.lastControlWord, word); + printMatchData(); + this.lastControlWord = word; + } + + private void printTransition(NtControlWord old, NtControlWord newWord) { + logger.info("ROBOT TRANSITIONED MODES! From " + old.toString() + " to " + newWord.toString()); + } + + private void printMatchData() { + // this information seems to be published at the same time + String event = eventName.get(); + if (event.isBlank()) { + // nothing to log + return; + } + String type = + switch ((int) matchType.get()) { + case 1 -> "P"; + case 2 -> "Q"; + case 3 -> "E"; + default -> ""; + }; + var match = String.valueOf(matchNumber.get()); + var replay = replayNumber.get(); + + var station = (isRedAlliance.get() ? "RED" : "BLUE") + stationNumber.get(); + + var message = + "Event: " + + event + + ", Match: " + + type + + match + + ", Replay: " + + replay + + ", Station: " + + station; + logger.info(message); + } + + // Copied from + // https://github.com/wpilibsuite/allwpilib/blob/07192285f65321a2f7363227a2216f09b715252d/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java#L123C1-L140C4 + // TODO: upstream! + /** + * Gets the current control word of the driver station. + * + *

The control work contains the robot state. + * + * @param controlWord the ControlWord to update + * @return + * @see "HAL_GetControlWord" + */ + private static NtControlWord getControlWord(long word) { + return new NtControlWord( + (word & 1) != 0, + ((word >> 1) & 1) != 0, + ((word >> 2) & 1) != 0, + ((word >> 3) & 1) != 0, + ((word >> 4) & 1) != 0, + ((word >> 5) & 1) != 0); + } +} diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index 5765972f91..2ab20defb5 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -58,6 +58,8 @@ public class NetworkTablesManager { private final TimeSyncManager m_timeSync = new TimeSyncManager(kRootTable); + NTDriverStation ntDriverStation; + private NetworkTablesManager() { ntInstance.addLogger( LogMessage.kInfo, LogMessage.kCritical, this::logNtMessage); // to hide error messages @@ -66,6 +68,8 @@ private NetworkTablesManager() { ntInstance.addListener( m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged); + ntDriverStation = new NTDriverStation(this.getNTInst()); + // Get the UI state in sync with the backend. NT should fire a callback when it first connects // to the robot broadcastConnectedStatus();