diff --git a/docs/docs/installation/index.md b/docs/docs/installation/index.md index 42202a92..3d8e2b2f 100644 --- a/docs/docs/installation/index.md +++ b/docs/docs/installation/index.md @@ -38,16 +38,6 @@ repositories { mavenLocal() } -configurations.all { - exclude group: "edu.wpi.first.wpilibj" -} - -task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { - mainClass = "org.littletonrobotics.junction.CheckInstall" - classpath = sourceSets.main.runtimeClasspath -} -compileJava.finalizedBy checkAkitInstall - dependencies { // ... def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) diff --git a/example_projects/wpilib_build_file/build.gradle b/example_projects/wpilib_build_file/build.gradle index a3a6700f..7e00e4c5 100644 --- a/example_projects/wpilib_build_file/build.gradle +++ b/example_projects/wpilib_build_file/build.gradle @@ -61,16 +61,6 @@ repositories { mavenLocal() } -configurations.all { - exclude group: "edu.wpi.first.wpilibj" -} - -task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { - mainClass = "org.littletonrobotics.junction.CheckInstall" - classpath = sourceSets.main.runtimeClasspath -} -compileJava.finalizedBy checkAkitInstall - // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. dependencies { diff --git a/junction/core/src/org/littletonrobotics/junction/CheckInstall.java b/junction/core/src/org/littletonrobotics/junction/CheckInstall.java deleted file mode 100644 index 8e99d27c..00000000 --- a/junction/core/src/org/littletonrobotics/junction/CheckInstall.java +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// 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. - -package org.littletonrobotics.junction; - -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.util.WPILibVersion; -import java.io.File; -import java.io.FileInputStream; -import java.util.regex.Pattern; - -public class CheckInstall { - private CheckInstall() { - } - - public static void main(String[] args) { - run(); - } - - /** - * Checks that the version of the installed WPILib shim matches the version of - * WPILib in "build.gradle". A mismatch indicates that AdvantageKit is being - * used with the wrong version of WPILib. If the check fails, a message will be - * printed to stderr and the program will exit. - * - *

- * This method cannot run on the roboRIO, only in simulation. - */ - static void run() { - String akitWPILibVersion = WPILibVersion.Version; - String installedWPILibVersion = ""; - - var buildFile = new File(Filesystem.getLaunchDirectory(), "build.gradle"); - try (FileInputStream inputStream = new FileInputStream(buildFile)) { - String contents = new String(inputStream.readAllBytes()); - for (String line : contents.split("\n")) { - if (line.contains("\"edu.wpi.first.GradleRIO\"")) { - var matcher = Pattern.compile("\"[^\"]+\"").matcher(line); - while (matcher.find()) { - int start = matcher.start() + 1; - int end = matcher.end() - 1; - if (end >= start) { - installedWPILibVersion = line.substring(start, end); - } - } - break; - } - } - } catch (Exception e) { - } - - if (!akitWPILibVersion.equals(installedWPILibVersion)) { - System.err.println( - "The version of AdvantageKit installed in this project requires WPILib " - + akitWPILibVersion - + ", but " - + (installedWPILibVersion.length() == 0 - ? "an unknown version of WPILib" - : "WPILib " + installedWPILibVersion) - + " is currently installed. Please update AdvantageKit and/or WPILib to compatible versions (the supported version of WPILib is listed in the release notes for each AdvantageKit version). DO NOT override this check; running with invalid versions will result in a broken project with issues that are difficult to diagnose.\n\n*** EXITING DUE TO INVALID ADVANTAGEKIT INSTALLATION, SEE ABOVE. ***"); - System.exit(1); - } - } -} \ No newline at end of file diff --git a/junction/core/src/org/littletonrobotics/junction/LoggedRobot.java b/junction/core/src/org/littletonrobotics/junction/LoggedRobot.java index f6fcf45b..81e03e45 100644 --- a/junction/core/src/org/littletonrobotics/junction/LoggedRobot.java +++ b/junction/core/src/org/littletonrobotics/junction/LoggedRobot.java @@ -73,11 +73,6 @@ protected void finalize() { @Override @SuppressWarnings("UnsafeFinalization") public void startCompetition() { - // Check for invalid AdvantageKit install in sim - if (isSimulation()) { - CheckInstall.run(); - } - // Robot init methods long initStart = Logger.getRealTimestamp(); robotInit(); @@ -141,7 +136,7 @@ private static final class GcStatsCollector { private List gcBeans = ManagementFactory.getGarbageCollectorMXBeans(); private final long[] lastTimes = new long[gcBeans.size()]; private final long[] lastCounts = new long[gcBeans.size()]; - + public void update() { long accumTime = 0; long accumCounts = 0; @@ -150,11 +145,11 @@ public void update() { long gcCount = gcBeans.get(i).getCollectionCount(); accumTime += gcTime - lastTimes[i]; accumCounts += gcCount - lastCounts[i]; - + lastTimes[i] = gcTime; lastCounts[i] = gcCount; } - + Logger.recordOutput("LoggedRobot/GCTimeMS", (double) accumTime); Logger.recordOutput("LoggedRobot/GCCounts", (double) accumCounts); } diff --git a/junction/core/src/org/littletonrobotics/junction/Logger.java b/junction/core/src/org/littletonrobotics/junction/Logger.java index 569dba18..3f5bbd79 100644 --- a/junction/core/src/org/littletonrobotics/junction/Logger.java +++ b/junction/core/src/org/littletonrobotics/junction/Logger.java @@ -263,13 +263,18 @@ static void periodicBeforeUser() { } } + // Update Driver Station + long dsStart = getRealTimestamp(); + if (hasReplaySource()) { + LoggedDriverStation.replayFromLog(entry.getSubtable("DriverStation")); + } + // Update default inputs long saveDataStart = getRealTimestamp(); - LoggedDriverStation.periodic(); - LoggedSystemStats.periodic(); + LoggedSystemStats.periodic(entry.getSubtable("SystemStats")); LoggedPowerDistribution loggedPowerDistribution = LoggedPowerDistribution.getInstance(); if (loggedPowerDistribution != null) { - loggedPowerDistribution.periodic(); + loggedPowerDistribution.periodic(entry.getSubtable("PowerDistribution")); } for (int i = 0; i < dashboardInputs.size(); i++) { dashboardInputs.get(i).periodic(); @@ -300,18 +305,12 @@ static void periodicBeforeUser() { long saveDataEnd = getRealTimestamp(); // Log output data + if (hasReplaySource()) { + recordOutput("Logger/DriverStationPeriodicMS", (saveDataEnd - saveDataStart) / 1000.0); + } recordOutput("Logger/ConduitPeriodicMS", (conduitCaptureEnd - conduitCaptureStart) / 1000.0); - recordOutput("Logger/SavePeriodicMS", (saveDataEnd - saveDataStart) / 1000.0); + recordOutput("Logger/SavePeriodicMS", (saveDataStart - dsStart) / 1000.0); recordOutput("Logger/QueuedCycles", receiverQueue.size()); - } else { - // Retrieve new data even if logger is disabled - ConduitApi.getInstance().captureData(); - LoggedDriverStation.periodic(); - LoggedPowerDistribution loggedPowerDistribution = LoggedPowerDistribution.getInstance(); - if (loggedPowerDistribution != null) { - loggedPowerDistribution.periodic(); - } - LoggedSystemStats.periodic(); } } @@ -322,6 +321,12 @@ static void periodicBeforeUser() { */ static void periodicAfterUser(long userCodeLength, long periodicBeforeLength) { if (running) { + // Update Driver Station + long dsStart = getRealTimestamp(); + if (!hasReplaySource()) { + LoggedDriverStation.saveToLog(entry.getSubtable("DriverStation")); + } + // Update final outputs long autoLogStart = getRealTimestamp(); AutoLogOutputManager.periodic(); @@ -337,6 +342,9 @@ static void periodicAfterUser(long userCodeLength, long periodicBeforeLength) { long consoleCaptureEnd = getRealTimestamp(); // Record timing data + if (!hasReplaySource()) { + recordOutput("Logger/DriverStationPeriodicMS", (autoLogStart - dsStart) / 1000.0); + } recordOutput("Logger/AutoLogPeriodicMS", (alertLogStart - autoLogStart) / 1000.0); recordOutput("Logger/AlertLogPeriodicMS", (consoleCaptureStart - alertLogStart) / 1000.0); recordOutput("Logger/ConsolePeriodicMS", (consoleCaptureEnd - consoleCaptureStart) / 1000.0); diff --git a/junction/core/src/org/littletonrobotics/junction/inputs/LoggedDriverStation.java b/junction/core/src/org/littletonrobotics/junction/inputs/LoggedDriverStation.java index ea2a8c3a..353c4554 100644 --- a/junction/core/src/org/littletonrobotics/junction/inputs/LoggedDriverStation.java +++ b/junction/core/src/org/littletonrobotics/junction/inputs/LoggedDriverStation.java @@ -17,318 +17,112 @@ import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.hal.DriverStationJNI; -import edu.wpi.first.networktables.BooleanPublisher; -import edu.wpi.first.networktables.IntegerPublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.MatchType; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; /** * Manages logging and replaying data from the driver station (robot state, * joysticks, etc.) */ public class LoggedDriverStation { - private static final DriverStationInputs dsInputs = new DriverStationInputs(); - private static final JoystickInputs[] joystickInputs = { new JoystickInputs(), new JoystickInputs(), - new JoystickInputs(), - new JoystickInputs(), new JoystickInputs(), new JoystickInputs() }; - private static final MatchDataSender matchDataSender = new MatchDataSender(); - private LoggedDriverStation() { } - /** - * General driver station data that needs to be updated throughout the match. - */ - public static class DriverStationInputs implements LoggableInputs { - public int allianceStation = 0; - public String eventName = ""; - public String gameSpecificMessage = ""; - public int matchNumber = 0; - public int replayNumber = 0; - public int matchType = 0; - public double matchTime = -1.0; - - public boolean enabled = false; - public boolean autonomous = false; - public boolean test = false; - public boolean emergencyStop = false; - public boolean fmsAttached = false; - public boolean dsAttached = false; - - public void toLog(LogTable table) { - table.put("AllianceStation", allianceStation); - table.put("EventName", eventName); - table.put("GameSpecificMessage", gameSpecificMessage); - table.put("MatchNumber", matchNumber); - table.put("ReplayNumber", replayNumber); - table.put("MatchType", matchType); - table.put("MatchTime", matchTime); - - table.put("Enabled", enabled); - table.put("Autonomous", autonomous); - table.put("Test", test); - table.put("EmergencyStop", emergencyStop); - table.put("FMSAttached", fmsAttached); - table.put("DSAttached", dsAttached); - } - - public void fromLog(LogTable table) { - allianceStation = table.get("AllianceStation", allianceStation); - eventName = table.get("EventName", eventName); - gameSpecificMessage = table.get("GameSpecificMessage", gameSpecificMessage); - matchNumber = table.get("MatchNumber", matchNumber); - replayNumber = table.get("ReplayNumber", replayNumber); - matchType = table.get("MatchType", matchType); - matchTime = table.get("MatchTime", matchTime); - - enabled = table.get("Enabled", enabled); - autonomous = table.get("Autonomous", autonomous); - test = table.get("Test", test); - emergencyStop = table.get("EmergencyStop", emergencyStop); - fmsAttached = table.get("FMSAttached", fmsAttached); - dsAttached = table.get("DSAttached", dsAttached); + /** Save the current DS state to the log table. */ + public static void saveToLog(LogTable table) { + ConduitApi conduit = ConduitApi.getInstance(); + + table.put("AllianceStation", conduit.getAllianceStation()); + table.put("EventName", conduit.getEventName().trim()); + table.put("GameSpecificMessage", conduit.getGameSpecificMessage().trim()); + table.put("MatchNumber", conduit.getMatchNumber()); + table.put("ReplayNumber", conduit.getReplayNumber()); + table.put("MatchType", conduit.getMatchType()); + table.put("MatchTime", conduit.getMatchTime()); + + int controlWord = conduit.getControlWord(); + table.put("Enabled", (controlWord & 1) != 0); + table.put("Autonomous", (controlWord & 2) != 0); + table.put("Test", (controlWord & 4) != 0); + table.put("EmergencyStop", (controlWord & 8) != 0); + table.put("FMSAttached", (controlWord & 16) != 0); + table.put("DSAttached", (controlWord & 32) != 0); + + for (int id = 0; id < DriverStation.kJoystickPorts; id++) { + LogTable joystickTable = table.getSubtable("Joystick" + Integer.toString(id)); + joystickTable.put("Name", conduit.getJoystickName(id).trim()); + joystickTable.put("Type", conduit.getJoystickType(id)); + joystickTable.put("Xbox", conduit.isXbox(id)); + joystickTable.put("ButtonCount", conduit.getButtonCount(id)); + joystickTable.put("ButtonValues", conduit.getButtonValues(id)); + + int povCount = conduit.getPovCount(id); + int[] povValues = new int[povCount]; + System.arraycopy(conduit.getPovValues(id), 0, povValues, 0, povCount); + joystickTable.put("POVs", povValues); + + int axisCount = conduit.getAxisCount(id); + float[] axisValues = new float[axisCount]; + int[] axisTypes = new int[axisCount]; + System.arraycopy(conduit.getAxisValues(id), 0, axisValues, 0, axisCount); + System.arraycopy(conduit.getAxisTypes(id), 0, axisTypes, 0, axisCount); + joystickTable.put("AxisValues", axisValues); + joystickTable.put("AxisTypes", axisTypes); } } - /** - * All of the required inputs for a single joystick. - */ - public static class JoystickInputs implements LoggableInputs { - public String name = ""; - public int type = 0; - public boolean xbox = false; - public int buttonCount = 0; - public int buttonValues = 0; - public int[] povs = {}; - public float[] axisValues = {}; - public int[] axisTypes = {}; - - public void toLog(LogTable table) { - table.put("Name", name); - table.put("Type", type); - table.put("Xbox", xbox); - table.put("ButtonCount", buttonCount); - table.put("ButtonValues", buttonValues); - table.put("POVs", povs); - table.put("AxisValues", axisValues); - table.put("AxisTypes", axisTypes); - } - - public void fromLog(LogTable table) { - name = table.get("Name", name); - type = table.get("Type", type); - xbox = table.get("Xbox", xbox); - buttonCount = table.get("ButtonCount", buttonCount); - buttonValues = table.get("ButtonValues", buttonValues); - povs = table.get("POVs", povs); - axisValues = table.get("AxisValues", axisValues); - axisTypes = table.get("AxisTypes", axisTypes); - } - } - - /** - * Records inputs from the real driver station via conduit - */ - public static void periodic() { - // Update inputs from conduit - if (!Logger.hasReplaySource()) { - ConduitApi conduit = ConduitApi.getInstance(); - - dsInputs.allianceStation = conduit.getAllianceStation(); - dsInputs.eventName = conduit.getEventName().trim(); - dsInputs.gameSpecificMessage = conduit.getGameSpecificMessage().trim(); - dsInputs.matchNumber = conduit.getMatchNumber(); - dsInputs.replayNumber = conduit.getReplayNumber(); - dsInputs.matchType = conduit.getMatchType(); - dsInputs.matchTime = conduit.getMatchTime(); - - int controlWord = conduit.getControlWord(); - dsInputs.enabled = (controlWord & 1) != 0; - dsInputs.autonomous = (controlWord & 2) != 0; - dsInputs.test = (controlWord & 4) != 0; - dsInputs.emergencyStop = (controlWord & 8) != 0; - dsInputs.fmsAttached = (controlWord & 16) != 0; - dsInputs.dsAttached = (controlWord & 32) != 0; - - for (int id = 0; id < joystickInputs.length; id++) { - JoystickInputs joystick = joystickInputs[id]; - joystick.name = conduit.getJoystickName(id).trim(); - joystick.type = conduit.getJoystickType(id); - joystick.xbox = conduit.isXbox(id); - joystick.buttonCount = conduit.getButtonCount(id); - joystick.buttonValues = conduit.getButtonValues(id); - - // POVs - int povCount = conduit.getPovCount(id); - int[] povValues = conduit.getPovValues(id); - joystick.povs = new int[povCount]; - for (int i = 0; i < povCount; i++) { - joystick.povs[i] = povValues[i]; - } - - // Axes - int axisCount = conduit.getAxisCount(id); - float[] axisValues = conduit.getAxisValues(id); - int[] axisTypes = conduit.getAxisTypes(id); - joystick.axisValues = new float[axisCount]; - joystick.axisTypes = new int[axisCount]; - for (int i = 0; i < axisCount; i++) { - joystick.axisValues[i] = axisValues[i]; - joystick.axisTypes[i] = axisTypes[i]; - } + /** Read the DS state to the log table and publish to the HAL sim. */ + public static void replayFromLog(LogTable table) { + DriverStationSim.setAllianceStationId( + switch (table.get("AllianceStation", 0)) { + case DriverStationJNI.kRed1AllianceStation -> AllianceStationID.Red1; + case DriverStationJNI.kRed2AllianceStation -> AllianceStationID.Red2; + case DriverStationJNI.kRed3AllianceStation -> AllianceStationID.Red3; + case DriverStationJNI.kBlue1AllianceStation -> AllianceStationID.Blue1; + case DriverStationJNI.kBlue2AllianceStation -> AllianceStationID.Blue2; + case DriverStationJNI.kBlue3AllianceStation -> AllianceStationID.Blue3; + default -> AllianceStationID.Unknown; + }); + DriverStationSim.setEventName(table.get("EventName", "")); + DriverStationSim.setGameSpecificMessage(table.get("GameSpecificMessage", "")); + DriverStationSim.setMatchNumber(table.get("MatchNumber", 0)); + DriverStationSim.setReplayNumber(table.get("ReplayNumber", 0)); + DriverStationSim.setMatchType(MatchType.values()[table.get("MatchType", 0)]); + DriverStationSim.setMatchTime(table.get("MatchTime", -1.0)); + + DriverStationSim.setEnabled(table.get("Enabled", false)); + DriverStationSim.setAutonomous(table.get("Autonomous", false)); + DriverStationSim.setTest(table.get("Test", false)); + DriverStationSim.setEStop(table.get("EmergencyStop", false)); + DriverStationSim.setFmsAttached(table.get("FMSAttached", false)); + DriverStationSim.setDsAttached(table.get("DSAttached", false)); + + for (int id = 0; id < DriverStation.kJoystickPorts; id++) { + LogTable joystickTable = table.getSubtable("Joystick" + Integer.toString(id)); + DriverStationSim.setJoystickName(id, joystickTable.get("Name", "")); + DriverStationSim.setJoystickType(id, joystickTable.get("Type", 0)); + DriverStationSim.setJoystickIsXbox(id, joystickTable.get("Xbox", false)); + DriverStationSim.setJoystickButtonCount(id, joystickTable.get("ButtonCount", 0)); + DriverStationSim.setJoystickButtons(id, joystickTable.get("ButtonValues", 0)); + + int[] povValues = joystickTable.get("POVs", new int[0]); + DriverStationSim.setJoystickPOVCount(id, povValues.length); + for (int i = 0; i < povValues.length; i++) { + DriverStationSim.setJoystickPOV(id, i, povValues[i]); } - } - - // Send/receive log data - Logger.processInputs("DriverStation", dsInputs); - for (int id = 0; id < joystickInputs.length; id++) { - Logger.processInputs("DriverStation/Joystick" + Integer.toString(id), joystickInputs[id]); - } - // Update FMSInfo table - matchDataSender.sendMatchData(dsInputs); - } - - /** - * Returns a reference to an object containing all driver station data other - * than joysticks. - */ - public static DriverStationInputs getDSData() { - return dsInputs; - } - - /** - * Returns a reference to an object containing data for a single joystick. - * - * @param id ID of the joystick to read(0-6) - */ - public static JoystickInputs getJoystickData(int id) { - return joystickInputs[id]; - } - - /** - * Class for updating the "FMSInfo" table in NetworkTables, modified from the - * original DriverStation. - */ - private static class MatchDataSender { - NetworkTable table; - StringPublisher typeMetadata; - StringPublisher gameSpecificMessage; - StringPublisher eventName; - IntegerPublisher matchNumber; - IntegerPublisher replayNumber; - IntegerPublisher matchType; - BooleanPublisher alliance; - IntegerPublisher station; - IntegerPublisher controlWord; - boolean oldIsRedAlliance = true; - long oldStationNumber = 1; - String oldEventName = ""; - String oldGameSpecificMessage = ""; - long oldMatchNumber; - long oldReplayNumber; - long oldMatchType; - long oldControlWord; - - MatchDataSender() { - table = NetworkTableInstance.getDefault().getTable("FMSInfo"); - typeMetadata = table.getStringTopic(".type").publish(); - typeMetadata.set("FMSInfo"); - gameSpecificMessage = table.getStringTopic("GameSpecificMessage").publish(); - gameSpecificMessage.set(""); - eventName = table.getStringTopic("EventName").publish(); - eventName.set(""); - matchNumber = table.getIntegerTopic("MatchNumber").publish(); - matchNumber.set(0); - replayNumber = table.getIntegerTopic("ReplayNumber").publish(); - replayNumber.set(0); - matchType = table.getIntegerTopic("MatchType").publish(); - matchType.set(0); - alliance = table.getBooleanTopic("IsRedAlliance").publish(); - alliance.set(true); - station = table.getIntegerTopic("StationNumber").publish(); - station.set(1); - controlWord = table.getIntegerTopic("FMSControlData").publish(); - controlWord.set(0); - } - - private void sendMatchData(DriverStationInputs dsInputs) { - boolean isRedAlliance = false; - int stationNumber = 1; - switch (dsInputs.allianceStation) { - case DriverStationJNI.kRed1AllianceStation: - isRedAlliance = true; - stationNumber = 1; - break; - case DriverStationJNI.kRed2AllianceStation: - isRedAlliance = true; - stationNumber = 2; - break; - case DriverStationJNI.kRed3AllianceStation: - isRedAlliance = true; - stationNumber = 3; - break; - case DriverStationJNI.kBlue1AllianceStation: - isRedAlliance = false; - stationNumber = 1; - break; - case DriverStationJNI.kBlue2AllianceStation: - isRedAlliance = false; - stationNumber = 2; - break; - case DriverStationJNI.kBlue3AllianceStation: - isRedAlliance = false; - stationNumber = 3; - break; - } - - String currentEventName = dsInputs.eventName; - String currentGameSpecificMessage = dsInputs.gameSpecificMessage; - long currentMatchNumber = dsInputs.matchNumber; - long currentReplayNumber = dsInputs.replayNumber; - long currentMatchType = dsInputs.matchType; - long currentControlWord = 0; - currentControlWord += dsInputs.enabled ? 1 : 0; - currentControlWord += dsInputs.autonomous ? 2 : 0; - currentControlWord += dsInputs.test ? 4 : 0; - currentControlWord += dsInputs.emergencyStop ? 8 : 0; - currentControlWord += dsInputs.fmsAttached ? 16 : 0; - currentControlWord += dsInputs.dsAttached ? 32 : 0; - - if (oldIsRedAlliance != isRedAlliance) { - alliance.set(isRedAlliance); - oldIsRedAlliance = isRedAlliance; - } - if (oldStationNumber != stationNumber) { - station.set(stationNumber); - oldStationNumber = stationNumber; - } - if (!oldEventName.equals(currentEventName)) { - eventName.set(currentEventName); - oldEventName = currentEventName; - } - if (!oldGameSpecificMessage.equals(currentGameSpecificMessage)) { - gameSpecificMessage.set(currentGameSpecificMessage); - oldGameSpecificMessage = currentGameSpecificMessage; - } - if (currentMatchNumber != oldMatchNumber) { - matchNumber.set(currentMatchNumber); - oldMatchNumber = currentMatchNumber; - } - if (currentReplayNumber != oldReplayNumber) { - replayNumber.set(currentReplayNumber); - oldReplayNumber = currentReplayNumber; - } - if (currentMatchType != oldMatchType) { - matchType.set(currentMatchType); - oldMatchType = currentMatchType; - } - if (currentControlWord != oldControlWord) { - controlWord.set(currentControlWord); - oldControlWord = currentControlWord; + float[] axisValues = joystickTable.get("AxisValues", new float[0]); + int[] axisTypes = joystickTable.get("AxisTypes", new int[0]); + DriverStationSim.setJoystickAxisCount(id, axisValues.length); + for (int i = 0; i < axisValues.length; i++) { + DriverStationSim.setJoystickAxis(id, i, axisValues[i]); + DriverStationSim.setJoystickAxisType(id, i, axisTypes[i]); } } + + DriverStationSim.notifyNewData(); } } diff --git a/junction/core/src/org/littletonrobotics/junction/inputs/LoggedPowerDistribution.java b/junction/core/src/org/littletonrobotics/junction/inputs/LoggedPowerDistribution.java index a718eebc..fd8f550d 100644 --- a/junction/core/src/org/littletonrobotics/junction/inputs/LoggedPowerDistribution.java +++ b/junction/core/src/org/littletonrobotics/junction/inputs/LoggedPowerDistribution.java @@ -27,8 +27,6 @@ public class LoggedPowerDistribution { private static LoggedPowerDistribution instance; - private final PowerDistributionInputs pdpInputs = new PowerDistributionInputs(); - private int moduleID; private int moduleType; @@ -61,75 +59,23 @@ public static LoggedPowerDistribution getInstance(int moduleID, PowerDistributio return instance; } - public static class PowerDistributionInputs implements LoggableInputs { - public double pdpTemperature = 0.0; - public double pdpVoltage = 12.0; - public double[] pdpChannelCurrents = new double[24]; - public double pdpTotalCurrent = 0.0; - public double pdpTotalPower = 0.0; - public double pdpTotalEnergy = 0.0; - - public int channelCount = 24; - public int handle = 0; - public int type = 0; - public int moduleId = 0; - public long faults = 0; - public long stickyFaults = 0; - - @Override - public void toLog(LogTable table) { - table.put("Temperature", pdpTemperature); - table.put("Voltage", pdpVoltage); - table.put("ChannelCurrent", pdpChannelCurrents); - table.put("TotalCurrent", pdpTotalCurrent); - table.put("TotalPower", pdpTotalPower); - table.put("TotalEnergy", pdpTotalEnergy); - - table.put("ChannelCount", channelCount); - table.put("Faults", faults); - table.put("StickyFaults", stickyFaults); - } - - @Override - public void fromLog(LogTable table) { - pdpTemperature = table.get("Temperature", pdpTemperature); - pdpVoltage = table.get("Voltage", pdpVoltage); - pdpChannelCurrents = table.get("ChannelCurrent", pdpChannelCurrents); - pdpTotalCurrent = table.get("TotalCurrent", pdpTotalCurrent); - pdpTotalPower = table.get("TotalPower", pdpTotalPower); - pdpTotalEnergy = table.get("TotalEnergy", pdpTotalEnergy); - - channelCount = table.get("ChannelCount", channelCount); - faults = table.get("Faults", faults); - stickyFaults = table.get("StickyFaults", stickyFaults); - } - } - - public void periodic() { - // Update inputs from conduit + public void periodic(LogTable table) { if (!Logger.hasReplaySource()) { ConduitApi conduit = ConduitApi.getInstance(); - pdpInputs.pdpTemperature = conduit.getPDPTemperature(); - pdpInputs.pdpVoltage = conduit.getPDPVoltage(); + table.put("Temperature", conduit.getPDPTemperature()); + table.put("Voltage", conduit.getPDPVoltage()); + double[] currents = new double[24]; for (int i = 0; i < 24; i++) { - pdpInputs.pdpChannelCurrents[i] = conduit.getPDPChannelCurrent(i); + currents[i] = conduit.getPDPChannelCurrent(i); } - pdpInputs.pdpTotalCurrent = conduit.getPDPTotalCurrent(); - pdpInputs.pdpTotalPower = conduit.getPDPTotalPower(); - pdpInputs.pdpTotalEnergy = conduit.getPDPTotalEnergy(); - pdpInputs.channelCount = conduit.getPDPChannelCount(); - pdpInputs.handle = conduit.getPDPHandle(); - pdpInputs.type = conduit.getPDPType(); - pdpInputs.moduleId = conduit.getPDPModuleId(); - pdpInputs.faults = conduit.getPDPFaults(); - pdpInputs.stickyFaults = conduit.getPDPStickyFaults(); + table.put("ChannelCurrent", currents); + table.put("TotalCurrent", conduit.getPDPTotalCurrent()); + table.put("TotalPower", conduit.getPDPTotalPower()); + table.put("TotalEnergy", conduit.getPDPTotalEnergy()); + + table.put("ChannelCount", conduit.getPDPChannelCount()); + table.put("Faults", conduit.getPDPFaults()); + table.put("StickyFaults", conduit.getPDPStickyFaults()); } - - Logger.processInputs("PowerDistribution", pdpInputs); } - - public PowerDistributionInputs getInputs() { - return pdpInputs; - } - } diff --git a/junction/core/src/org/littletonrobotics/junction/inputs/LoggedSystemStats.java b/junction/core/src/org/littletonrobotics/junction/inputs/LoggedSystemStats.java index 2dc6a769..82aaec10 100644 --- a/junction/core/src/org/littletonrobotics/junction/inputs/LoggedSystemStats.java +++ b/junction/core/src/org/littletonrobotics/junction/inputs/LoggedSystemStats.java @@ -17,190 +17,58 @@ import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.hal.can.CANStatus; - /** * Manages logging general system data. */ public class LoggedSystemStats { - private static final SystemStatsInputs sysInputs = new SystemStatsInputs(); - private LoggedSystemStats() { } - public static class SystemStatsInputs implements LoggableInputs { - public int fpgaVersion = 0; - public int fpgaRevision = 0; - public String serialNumber = ""; - public String comments = ""; - public int teamNumber = 0; - public boolean fpgaButton = false; - public boolean systemActive = false; - public boolean brownedOut = false; - public int commsDisableCount = 0; - public boolean rslState = false; - public boolean systemTimeValid = false; - public double voltageVin = 12.0; - public double currentVin = 0.0; - public double userVoltage3v3 = 3.3; - public double userCurrent3v3 = 0.0; - public boolean userActive3v3 = true; - public int userCurrentFaults3v3 = 0; - public double userVoltage5v = 5.0; - public double userCurrent5v = 0.0; - public boolean userActive5v = true; - public int userCurrentFaults5v = 0; - public double userVoltage6v = 6.0; - public double userCurrent6v = 0.0; - public boolean userActive6v = true; - public int userCurrentFaults6v = 0; - public double brownoutVoltage = 0.0; - public double cpuTemp = 0.0; - public CANStatus canStatus = new CANStatus(); - public long epochTime = 0; - - @Override - public void toLog(LogTable table) { - table.put("FPGAVersion", fpgaVersion); - table.put("FPGARevision", fpgaRevision); - table.put("SerialNumber", serialNumber); - table.put("Comments", comments); - table.put("TeamNumber", teamNumber); - table.put("FPGAButton", fpgaButton); - table.put("SystemActive", systemActive); - table.put("BrownedOut", brownedOut); - table.put("CommsDisableCount", commsDisableCount); - table.put("RSLState", rslState); - table.put("SystemTimeValid", systemTimeValid); - - table.put("BatteryVoltage", voltageVin); - table.put("BatteryCurrent", currentVin); - - table.put("3v3Rail/Voltage", userVoltage3v3); - table.put("3v3Rail/Current", userCurrent3v3); - table.put("3v3Rail/Active", userActive3v3); - table.put("3v3Rail/CurrentFaults", userCurrentFaults3v3); - - table.put("5vRail/Voltage", userVoltage5v); - table.put("5vRail/Current", userCurrent5v); - table.put("5vRail/Active", userActive5v); - table.put("5vRail/CurrentFaults", userCurrentFaults5v); - - table.put("6vRail/Voltage", userVoltage6v); - table.put("6vRail/Current", userCurrent6v); - table.put("6vRail/Active", userActive6v); - table.put("6vRail/CurrentFaults", userCurrentFaults6v); - - table.put("BrownoutVoltage", brownoutVoltage); - table.put("CPUTempCelsius", cpuTemp); - - table.put("CANBus/Utilization", canStatus.percentBusUtilization); - table.put("CANBus/OffCount", canStatus.busOffCount); - table.put("CANBus/TxFullCount", canStatus.txFullCount); - table.put("CANBus/ReceiveErrorCount", canStatus.receiveErrorCount); - table.put("CANBus/TransmitErrorCount", canStatus.transmitErrorCount); - - table.put("EpochTimeMicros", epochTime); - } - - @Override - public void fromLog(LogTable table) { - fpgaVersion = table.get("FPGAVersion", fpgaVersion); - fpgaRevision = table.get("FPGARevision", fpgaRevision); - serialNumber = table.get("SerialNumber", serialNumber); - comments = table.get("Comments", comments); - teamNumber = table.get("TeamNumber", teamNumber); - fpgaButton = table.get("FPGAButton", fpgaButton); - systemActive = table.get("SystemActive", systemActive); - commsDisableCount = table.get("CommsDisableCount", commsDisableCount); - brownedOut = table.get("BrownedOut", brownedOut); - rslState = table.get("RSLState", rslState); - systemTimeValid = table.get("SystemTimeValid", systemTimeValid); - - voltageVin = table.get("BatteryVoltage", voltageVin); - currentVin = table.get("BatteryCurrent", currentVin); - - userVoltage3v3 = table.get("3v3Rail/Voltage", userVoltage3v3); - userCurrent3v3 = table.get("3v3Rail/Current", userCurrent3v3); - userActive3v3 = table.get("3v3Rail/Active", userActive3v3); - userCurrentFaults3v3 = table.get("3v3Rail/CurrentFaults", userCurrentFaults3v3); - - userVoltage5v = table.get("5vRail/Voltage", userVoltage5v); - userCurrent5v = table.get("5vRail/Current", userCurrent5v); - userActive5v = table.get("5vRail/Active", userActive5v); - userCurrentFaults5v = table.get("5vRail/CurrentFaults", userCurrentFaults5v); - - userVoltage6v = table.get("6vRail/Voltage", userVoltage6v); - userCurrent6v = table.get("6vRail/Current", userCurrent6v); - userActive6v = table.get("6vRail/Active", userActive6v); - userCurrentFaults6v = table.get("6vRail/CurrentFaults", userCurrentFaults6v); - - brownoutVoltage = table.get("BrownoutVoltage", brownoutVoltage); - cpuTemp = table.get("CPUTempCelsius", cpuTemp); - - canStatus.setStatus( - table.get("CANBus/Utilization", canStatus.percentBusUtilization), - table.get("CANBus/OffCount", canStatus.busOffCount), - table.get("CANBus/TxFullCount", canStatus.txFullCount), - table.get("CANBus/ReceiveErrorCount", canStatus.receiveErrorCount), - table.get("CANBus/TransmitErrorCount", canStatus.transmitErrorCount)); - - epochTime = table.get("EpochTimeMicros", epochTime); - } - } - - public static void periodic() { + public static void periodic(LogTable table) { // Update inputs from conduit if (!Logger.hasReplaySource()) { ConduitApi conduit = ConduitApi.getInstance(); - sysInputs.fpgaVersion = conduit.getFPGAVersion(); - sysInputs.fpgaRevision = conduit.getFPGARevision(); - sysInputs.serialNumber = conduit.getSerialNumber(); - sysInputs.comments = conduit.getComments(); - sysInputs.teamNumber = conduit.getTeamNumber(); - sysInputs.fpgaButton = conduit.getFPGAButton(); - sysInputs.systemActive = conduit.getSystemActive(); - sysInputs.brownedOut = conduit.getBrownedOut(); - sysInputs.commsDisableCount = conduit.getCommsDisableCount(); - sysInputs.rslState = conduit.getRSLState(); - sysInputs.systemTimeValid = conduit.getSystemTimeValid(); - - sysInputs.voltageVin = conduit.getVoltageVin(); - sysInputs.currentVin = conduit.getCurrentVin(); - - sysInputs.userVoltage3v3 = conduit.getUserVoltage3v3(); - sysInputs.userCurrent3v3 = conduit.getUserCurrent3v3(); - sysInputs.userActive3v3 = conduit.getUserActive3v3(); - sysInputs.userCurrentFaults3v3 = conduit.getUserCurrentFaults3v3(); - - sysInputs.userVoltage5v = conduit.getUserVoltage5v(); - sysInputs.userCurrent5v = conduit.getUserCurrent5v(); - sysInputs.userActive5v = conduit.getUserActive5v(); - sysInputs.userCurrentFaults5v = conduit.getUserCurrentFaults5v(); - - sysInputs.userVoltage6v = conduit.getUserVoltage6v(); - sysInputs.userCurrent6v = conduit.getUserCurrent6v(); - sysInputs.userActive6v = conduit.getUserActive6v(); - sysInputs.userCurrentFaults6v = conduit.getUserCurrentFaults6v(); - - sysInputs.brownoutVoltage = conduit.getBrownoutVoltage(); - sysInputs.cpuTemp = conduit.getCPUTemp(); - - sysInputs.canStatus.setStatus( - conduit.getCANBusUtilization(), - (int) conduit.getBusOffCount(), - (int) conduit.getTxFullCount(), - (int) conduit.getReceiveErrorCount(), - (int) conduit.getTransmitErrorCount()); - - sysInputs.epochTime = conduit.getEpochTime(); + table.put("FPGAVersion", conduit.getFPGAVersion()); + table.put("FPGARevision", conduit.getFPGARevision()); + table.put("SerialNumber", conduit.getSerialNumber()); + table.put("Comments", conduit.getComments()); + table.put("TeamNumber", conduit.getTeamNumber()); + table.put("FPGAButton", conduit.getFPGAButton()); + table.put("SystemActive", conduit.getSystemActive()); + table.put("BrownedOut", conduit.getBrownedOut()); + table.put("CommsDisableCount", conduit.getCommsDisableCount()); + table.put("RSLState", conduit.getRSLState()); + table.put("SystemTimeValid", conduit.getSystemTimeValid()); + + table.put("BatteryVoltage", conduit.getVoltageVin()); + table.put("BatteryCurrent", conduit.getCurrentVin()); + + table.put("3v3Rail/Voltage", conduit.getUserVoltage3v3()); + table.put("3v3Rail/Current", conduit.getUserCurrent3v3()); + table.put("3v3Rail/Active", conduit.getUserActive3v3()); + table.put("3v3Rail/CurrentFaults", conduit.getUserCurrentFaults3v3()); + + table.put("5vRail/Voltage", conduit.getUserVoltage5v()); + table.put("5vRail/Current", conduit.getUserCurrent5v()); + table.put("5vRail/Active", conduit.getUserActive5v()); + table.put("5vRail/CurrentFaults", conduit.getUserCurrentFaults5v()); + + table.put("6vRail/Voltage", conduit.getUserVoltage6v()); + table.put("6vRail/Current", conduit.getUserCurrent6v()); + table.put("6vRail/Active", conduit.getUserActive6v()); + table.put("6vRail/CurrentFaults", conduit.getUserCurrentFaults6v()); + + table.put("BrownoutVoltage", conduit.getBrownoutVoltage()); + table.put("CPUTempCelsius", conduit.getCPUTemp()); + + table.put("CANBus/Utilization", conduit.getCANBusUtilization()); + table.put("CANBus/OffCount", conduit.getBusOffCount()); + table.put("CANBus/TxFullCount", conduit.getTxFullCount()); + table.put("CANBus/ReceiveErrorCount", conduit.getReceiveErrorCount()); + table.put("CANBus/TransmitErrorCount", conduit.getTransmitErrorCount()); + + table.put("EpochTimeMicros", conduit.getEpochTime()); } - - Logger.processInputs("SystemStats", sysInputs); - } - - public static SystemStatsInputs getInputs() { - return sysInputs; } } \ No newline at end of file diff --git a/publish_local.sh b/publish_local.sh index ba55360c..62e178e5 100755 --- a/publish_local.sh +++ b/publish_local.sh @@ -8,7 +8,7 @@ bazel run --define "publishing_javadoc=false" //conduit/api:api-export.publish bazel run --define "publishing_javadoc=false" //conduit/wpilibio:pom.publish bazel run --define "publishing_javadoc=false" //conduit/wpilibio:nativezip.publish -c opt bazel run --define "publishing_javadoc=false" //junction/core:core-export.publish -bazel run --define "publishing_javadoc=false" //junction/shims/wpilib:wpilib-export.publish +# bazel run --define "publishing_javadoc=false" //junction/shims/wpilib:wpilib-export.publish bazel run --define "publishing_javadoc=false" //junction/autolog:autolog-export.publish if [[ "$*" == athena ]] then