From 427d3c1e6554fa70b1d8dc641b92d8c5459eb313 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Thu, 19 Sep 2024 01:09:41 -0400 Subject: [PATCH 1/6] rework tracer --- .../wpi/first/wpilibj/IterativeRobotBase.java | 37 +- .../edu/wpi/first/wpilibj/TimedRobot.java | 4 + .../java/edu/wpi/first/wpilibj/Tracer.java | 328 ++++++++++++++---- .../java/edu/wpi/first/wpilibj/Watchdog.java | 24 -- .../edu/wpi/first/wpilibj/WatchdogTest.java | 30 -- 5 files changed, 270 insertions(+), 153 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index b0c46604283..d1207fc5ba1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -299,6 +299,8 @@ public double getPeriod() { /** Loop function. */ protected void loopFunc() { + Tracer.startTrace("RobotLoop"); + DriverStation.refreshData(); m_watchdog.reset(); @@ -344,15 +346,12 @@ protected void loopFunc() { switch (mode) { case kDisabled -> { disabledInit(); - m_watchdog.addEpoch("disabledInit()"); } case kAutonomous -> { autonomousInit(); - m_watchdog.addEpoch("autonomousInit()"); } case kTeleop -> { teleopInit(); - m_watchdog.addEpoch("teleopInit()"); } case kTest -> { if (m_lwEnabledInTest) { @@ -360,7 +359,6 @@ protected void loopFunc() { Shuffleboard.enableActuatorWidgets(); } testInit(); - m_watchdog.addEpoch("testInit()"); } default -> { // NOP @@ -374,44 +372,35 @@ protected void loopFunc() { switch (mode) { case kDisabled -> { DriverStationJNI.observeUserProgramDisabled(); - disabledPeriodic(); - m_watchdog.addEpoch("disabledPeriodic()"); + Tracer.traceFunc("DisabledPeriodic", this::disabledPeriodic); } case kAutonomous -> { DriverStationJNI.observeUserProgramAutonomous(); - autonomousPeriodic(); - m_watchdog.addEpoch("autonomousPeriodic()"); + Tracer.traceFunc("AutonomousPeriodic", this::autonomousPeriodic); } case kTeleop -> { DriverStationJNI.observeUserProgramTeleop(); - teleopPeriodic(); - m_watchdog.addEpoch("teleopPeriodic()"); + Tracer.traceFunc("TeleopPeriodic", this::teleopPeriodic); } case kTest -> { DriverStationJNI.observeUserProgramTest(); - testPeriodic(); - m_watchdog.addEpoch("testPeriodic()"); + Tracer.traceFunc("TestPeriodic", this::testPeriodic); } default -> { // NOP } } - robotPeriodic(); - m_watchdog.addEpoch("robotPeriodic()"); + Tracer.traceFunc("RobotPeriodic", this::robotPeriodic); SmartDashboard.updateValues(); - m_watchdog.addEpoch("SmartDashboard.updateValues()"); LiveWindow.updateValues(); - m_watchdog.addEpoch("LiveWindow.updateValues()"); Shuffleboard.update(); - m_watchdog.addEpoch("Shuffleboard.update()"); if (isSimulation()) { HAL.simPeriodicBefore(); - simulationPeriodic(); + Tracer.traceFunc("SimulationPeriodic", this::simulationPeriodic); HAL.simPeriodicAfter(); - m_watchdog.addEpoch("simulationPeriodic()"); } m_watchdog.disable(); @@ -421,15 +410,7 @@ protected void loopFunc() { NetworkTableInstance.getDefault().flushLocal(); } - // Warn on loop time overruns - if (m_watchdog.isExpired()) { - m_watchdog.printEpochs(); - } - } - - /** Prints list of epochs added so far and their times. */ - public void printWatchdogEpochs() { - m_watchdog.printEpochs(); + Tracer.endTrace(); } private void printLoopOverrunMessage() { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 7b59f528a6b..f514dd67957 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -128,6 +128,8 @@ public void startCompetition() { break; } + Tracer.startTrace("TimedRobot"); + callback.func.run(); // Increment the expiration time by the number of full periods it's behind @@ -150,6 +152,8 @@ public void startCompetition() { + (currentTime - callback.expirationTime) / callback.period * callback.period; m_callbacks.add(callback); } + + Tracer.endTrace(); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java index 15ca7fff81f..4723d2445b2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java @@ -4,81 +4,267 @@ package edu.wpi.first.wpilibj; +import java.lang.management.GarbageCollectorMXBean; +import java.lang.management.ManagementFactory; +import java.util.ArrayList; import java.util.HashMap; -import java.util.Map; -import java.util.function.Consumer; +import java.util.function.Supplier; +import java.util.concurrent.atomic.AtomicBoolean; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; /** - * A class for keeping track of how much time it takes for different parts of code to execute. This - * is done with epochs, that are added by calls to {@link #addEpoch(String)}, and can be printed - * with a call to {@link #printEpochs()}. - * - *

Epochs are a way to partition the time elapsed so that when overruns occur, one can determine - * which parts of an operation consumed the most time. + * A Utility class for tracing code execution time. + * Will put info to NetworkTables under the "Tracer" table. + * + *

Example inside {@code Robot.java} + * 


+ * @Override
+ * public void robotPeriodic() {
+ *   Tracer.traceFunc("CommandScheduler", scheduler::run);
+ *   Tracer.traceFunc("MyVendorDep", MyVendorDep::updateAll);
+ * }
+ *
+ * + *

Example inside a {@code Drive Subsystem} + * 


+ * public void periodic() {
+ *   Tracer.startTrace("DrivePeriodic");
+ *   for (var module : modules) {
+ *     Tracer.traceFunc("Module" + module.getName(), module::update);
+ *   }
+ *   Tracer.traceFunc("Gyro", gyro::update);
+ *   Tracer.endTrace();
+ * }
+ *
*/ public class Tracer { - private static final long kMinPrintPeriod = 1000000; // microseconds - - private long m_lastEpochsPrintTime; // microseconds - private long m_startTime; // microseconds - - private final Map m_epochs = new HashMap<>(); // microseconds - - /** Tracer constructor. */ - public Tracer() { - resetTimer(); - } - - /** Clears all epochs. */ - public void clearEpochs() { - m_epochs.clear(); - resetTimer(); - } - - /** Restarts the epoch timer. */ - public final void resetTimer() { - m_startTime = RobotController.getFPGATime(); - } - - /** - * Adds time since last epoch to the list printed by printEpochs(). - * - *

Epochs are a way to partition the time elapsed so that when overruns occur, one can - * determine which parts of an operation consumed the most time. - * - *

This should be called immediately after execution has finished, with a call to this method - * or {@link #resetTimer()} before execution. - * - * @param epochName The name to associate with the epoch. - */ - public void addEpoch(String epochName) { - long currentTime = RobotController.getFPGATime(); - m_epochs.put(epochName, currentTime - m_startTime); - m_startTime = currentTime; - } - - /** Prints list of epochs added so far and their times to the DriverStation. */ - public void printEpochs() { - printEpochs(out -> DriverStation.reportWarning(out, false)); - } - - /** - * Prints list of epochs added so far and their times to the entered String consumer. - * - *

This overload can be useful for logging to a file, etc. - * - * @param output the stream that the output is sent to - */ - public void printEpochs(Consumer output) { - long now = RobotController.getFPGATime(); - if (now - m_lastEpochsPrintTime > kMinPrintPeriod) { - StringBuilder sb = new StringBuilder(); - m_lastEpochsPrintTime = now; - m_epochs.forEach( - (key, value) -> sb.append(String.format("\t%s: %.6fs\n", key, value / 1.0e6))); - if (sb.length() > 0) { - output.accept(sb.toString()); - } + private static final class TraceStartData { + private double startTime = 0.0; + private double startGCTotalTime = 0.0; + + private void set(double startTime, double startGCTotalTime) { + this.startTime = startTime; + this.startGCTotalTime = startGCTotalTime; + } + } + + /** + * All of the tracers persistent state in a single object to be stored in a + * {@link ThreadLocal}. + */ + private static final class TracerState { + private final NetworkTable rootTable; + + // the stack of traces, every startTrace will add to this stack + // and every endTrace will remove from this stack + private final ArrayList traceStack = new ArrayList<>(); + // ideally we only need `traceStack` but in the interest of memory optimization + // and string concatenation speed we store the history of the stack to reuse the stack names + private final ArrayList traceStackHistory = new ArrayList<>(); + // the time of each trace, the key is the trace name, modified every endTrace + private final HashMap traceTimes = new HashMap<>(); + // the start time of each trace and the gc time at the start of the trace, + // the key is the trace name, modified every startTrace and endTrace. + private final HashMap traceStartTimes = new HashMap<>(); + // the publishers for each trace, the key is the trace name, modified every endCycle + private final HashMap publisherHeap = new HashMap<>(); + + // the garbage collector beans + private final ArrayList gcs = new ArrayList<>(ManagementFactory.getGarbageCollectorMXBeans()); + private final DoublePublisher gcTimeEntry; + private double gcTimeThisCycle = 0.0; + + private TracerState(String threadName) { + if (threadName == null) { + this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer"); + } else { + this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer").getSubTable(threadName); + } + this.gcTimeEntry = rootTable.getDoubleTopic("GCTime").publishEx( + "double", + "{ \"cached\": false}" + ); + } + + private String appendTraceStack(String trace) { + traceStack.add(trace); + StringBuilder sb = new StringBuilder(); + for (int i = 0; i < traceStack.size(); i++) { + sb.append(traceStack.get(i)); + if (i < traceStack.size() - 1) { + sb.append("/"); + } + } + String str = sb.toString(); + traceStackHistory.add(str); + return str; + } + + private String popTraceStack() { + traceStack.remove(traceStack.size() - 1); + return traceStackHistory.remove(traceStackHistory.size() - 1); + } + + private double totalGCTime() { + double gcTime = 0; + for (GarbageCollectorMXBean gc : gcs) { + gcTime += gc.getCollectionTime(); + } + return gcTime; + } + + private void endCycle() { + // update times for all already existing publishers + for (var publisher : publisherHeap.entrySet()) { + Double time = traceTimes.remove(publisher.getKey()); + if (time == null) time = 0.0; + publisher.getValue().set(time); + } + // create publishers for all new entries + for (var traceTime : traceTimes.entrySet()) { + DoublePublisher publisher = rootTable.getDoubleTopic(traceTime.getKey()).publishEx( + "double", + "{ \"cached\": false}" + ); + publisher.set(traceTime.getValue()); + publisherHeap.put(traceTime.getKey(), publisher); + } + + // log gc time + if (gcs.size() > 0) gcTimeEntry.set(gcTimeThisCycle); + gcTimeThisCycle = 0.0; + + // clean up state + traceTimes.clear(); + traceStackHistory.clear(); + } + } + + private static final AtomicBoolean singleThreadedMode = new AtomicBoolean(false); + private static final ThreadLocal threadLocalState = ThreadLocal.withInitial(() -> { + if (singleThreadedMode.get()) { + throw new IllegalStateException("Single threaded mode is enabled, cannot create new TracerState"); + } + return new TracerState(Thread.currentThread().getName()); + }); + + private static void startTraceInner(final String name, final TracerState state) { + String stack = state.appendTraceStack(name); + TraceStartData data = state.traceStartTimes.get(stack); + if (data == null) { + data = new TraceStartData(); + state.traceStartTimes.put(stack, data); + } + data.set(Timer.getFPGATimestamp() * 1_000.0, state.totalGCTime()); + } + + private static void endTraceInner(final TracerState state) { + try { + String stack = state.popTraceStack(); + var startData = state.traceStartTimes.get(stack); + double gcTimeSinceStart = state.totalGCTime() - startData.startGCTotalTime; + state.gcTimeThisCycle += gcTimeSinceStart; + state.traceTimes.put( + stack, + Timer.getFPGATimestamp() * 1_000.0 + - startData.startTime + - gcTimeSinceStart + ); + if (state.traceStack.size() == 0) { + state.endCycle(); + } + } catch (Exception e) { + DriverStation.reportError("[Tracer] An end trace was called with no opening trace " + e, true); + for (var trace : state.traceStack) { + DriverStation.reportError("[Tracer] Open trace: " + trace, false); + } + state.endCycle(); + } + } + + /** + * Starts a trace, + * should be called at the beginning of a function thats not being called by + * user code. + * Should be paired with {@link Tracer#endTrace()} at the end of the function. + * + * Best used in periodic functions in Subsystems and Robot.java. + * + * @param name the name of the trace, should be unique to the function. + */ + public static void startTrace(String name) { + startTraceInner(name, threadLocalState.get()); + } + + /** + * Ends a trace, should only be called at the end of a function thats not being + * called by user code. + * If a {@link Tracer#startTrace(String)} is not paired with a + * {@link Tracer#endTrace()} there could be a crash. + */ + public static void endTrace() { + endTraceInner(threadLocalState.get()); + } + + /** + * Disables garbage collection logging for the current thread. + * This can help performance in some cases. + */ + public static void disableGcLoggingForCurrentThread() { + TracerState state = threadLocalState.get(); + state.gcTimeEntry.close(); + state.gcs.clear(); + } + + /** + * Enables single threaded mode for the Tracer. + * This will cause traces on different threads to throw an exception. + * This will shorten the path of traced data in NetworkTables by not including the thread name. + * + *

Warning: This will throw an exception if called after any traces have been started. + */ + public static void enableSingleThreadedMode() { + threadLocalState.set(new TracerState(null)); + singleThreadedMode.set(true); + } + + /** + * Traces a function, should be used in place of + * {@link Tracer#startTrace(String)} and {@link Tracer#endTrace()} + * for functions called by user code like {@code CommandScheduler.run()} and + * other expensive functions. + * + * @param name the name of the trace, should be unique to the function. + * @param runnable the function to trace. + * + * @apiNote If you want to return a value then use + * {@link Tracer#traceFunc(String, Supplier)}. + */ + public static void traceFunc(String name, Runnable runnable) { + final TracerState state = threadLocalState.get(); + startTraceInner(name, state); + runnable.run(); + endTraceInner(state); + } + + /** + * Traces a function, should be used in place of + * {@link Tracer#startTrace(String)} and {@link Tracer#endTrace()} + * for functions called by user code like {@code CommandScheduler.run()} and + * other expensive functions. + * + * @param name the name of the trace, should be unique to the function. + * @param supplier the function to trace. + */ + public static R traceFunc(String name, Supplier supplier) { + final TracerState state = threadLocalState.get(); + startTraceInner(name, state); + R ret = supplier.get(); + endTraceInner(state); + return ret; } - } } + diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java index 6d00d6402bb..ad3c49d3446 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -31,8 +31,6 @@ public class Watchdog implements Closeable, Comparable { boolean m_suppressTimeoutMessage; - private final Tracer m_tracer; - private static final PriorityQueue m_watchdogs = new PriorityQueue<>(); private static ReentrantLock m_queueMutex = new ReentrantLock(); private static int m_notifier; @@ -52,7 +50,6 @@ public class Watchdog implements Closeable, Comparable { public Watchdog(double timeoutSeconds, Runnable callback) { m_timeoutSeconds = timeoutSeconds; m_callback = callback; - m_tracer = new Tracer(); } @Override @@ -94,7 +91,6 @@ public double getTime() { */ public void setTimeout(double timeoutSeconds) { m_startTimeSeconds = Timer.getFPGATimestamp(); - m_tracer.clearEpochs(); m_queueMutex.lock(); try { @@ -138,25 +134,6 @@ public boolean isExpired() { } } - /** - * Adds time since last epoch to the list printed by printEpochs(). - * - * @see Tracer#addEpoch(String) - * @param epochName The name to associate with the epoch. - */ - public void addEpoch(String epochName) { - m_tracer.addEpoch(epochName); - } - - /** - * Prints list of epochs added so far and their times. - * - * @see Tracer#printEpochs() - */ - public void printEpochs() { - m_tracer.printEpochs(); - } - /** * Resets the watchdog timer. * @@ -169,7 +146,6 @@ public void reset() { /** Enables the watchdog timer. */ public void enable() { m_startTimeSeconds = Timer.getFPGATimestamp(); - m_tracer.clearEpochs(); m_queueMutex.lock(); try { diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java index 5e9e1229418..4356903a340 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java @@ -117,36 +117,6 @@ void isExpiredTest() { } } - @Test - @ResourceLock("timing") - void epochsTest() { - final AtomicInteger watchdogCounter = new AtomicInteger(0); - - try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) { - // Run 1 - watchdog.enable(); - watchdog.addEpoch("Epoch 1"); - SimHooks.stepTiming(0.1); - watchdog.addEpoch("Epoch 2"); - SimHooks.stepTiming(0.1); - watchdog.addEpoch("Epoch 3"); - watchdog.disable(); - - assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); - - // Run 2 - watchdog.enable(); - watchdog.addEpoch("Epoch 1"); - SimHooks.stepTiming(0.2); - watchdog.reset(); - SimHooks.stepTiming(0.2); - watchdog.addEpoch("Epoch 2"); - watchdog.disable(); - - assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); - } - } - @Test @ResourceLock("timing") void multiWatchdogTest() { From 2294b0b8b63e4770a34ef5f2418ebf1ef0ff57d5 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Thu, 19 Sep 2024 01:12:24 -0400 Subject: [PATCH 2/6] fixed CommandScheduler for now --- .../first/wpilibj2/command/CommandScheduler.java | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 7e634cf5c92..2e924a66843 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -170,8 +170,6 @@ private void initCommand(Command command, Set requirements) { for (Consumer action : m_initActions) { action.accept(command); } - - m_watchdog.addEpoch(command.getName() + ".initialize()"); } /** @@ -264,7 +262,6 @@ public void run() { if (RobotBase.isSimulation()) { subsystem.simulationPeriodic(); } - m_watchdog.addEpoch(subsystem.getName() + ".periodic()"); } // Cache the active instance to avoid concurrency problems if setActiveLoop() is called from @@ -272,7 +269,6 @@ public void run() { EventLoop loopCache = m_activeButtonLoop; // Poll buttons for new commands to add. loopCache.poll(); - m_watchdog.addEpoch("buttons.run()"); m_inRunLoop = true; boolean isDisabled = RobotState.isDisabled(); @@ -289,7 +285,6 @@ public void run() { for (Consumer action : m_executeActions) { action.accept(command); } - m_watchdog.addEpoch(command.getName() + ".execute()"); if (command.isFinished()) { m_endingCommands.add(command); command.end(false); @@ -300,7 +295,6 @@ public void run() { iterator.remove(); m_requirements.keySet().removeAll(command.getRequirements()); - m_watchdog.addEpoch(command.getName() + ".end(false)"); } } m_inRunLoop = false; @@ -329,7 +323,6 @@ public void run() { m_watchdog.disable(); if (m_watchdog.isExpired()) { System.out.println("CommandScheduler loop overrun"); - m_watchdog.printEpochs(); } } @@ -487,7 +480,6 @@ private void cancel(Command command, Optional interruptor) { m_endingCommands.remove(command); m_scheduledCommands.remove(command); m_requirements.keySet().removeAll(command.getRequirements()); - m_watchdog.addEpoch(command.getName() + ".end(true)"); } /** Cancels all commands that are currently scheduled. */ @@ -530,11 +522,6 @@ public void enable() { m_disabled = false; } - /** Prints list of epochs added so far and their times. */ - public void printWatchdogEpochs() { - m_watchdog.printEpochs(); - } - /** * Adds an action to perform on the initialization of any command by the scheduler. * From 5d71f327ccf5398ec244d9900f5b28457b4f15e2 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Thu, 19 Sep 2024 14:06:31 -0400 Subject: [PATCH 3/6] added tunable values api --- .../edu/wpi/first/wpilibj/TunableValues.java | 204 ++++++++++++++++++ 1 file changed, 204 insertions(+) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/TunableValues.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TunableValues.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TunableValues.java new file mode 100644 index 00000000000..733d9c57030 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TunableValues.java @@ -0,0 +1,204 @@ +package edu.wpi.first.wpilibj; + +import java.util.HashMap; + +import edu.wpi.first.networktables.BooleanEntry; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.IntegerEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +/** + * An api for creating NT tunable values. + * Bools, Ints and Doubles are supported. + * + * @see TunableValues#getDouble(String, double) TunableValues.getDouble() + * @see TunableValues#getInteger(String, int) TunableValues.getInteger() + * @see TunableValues#getBoolean(String, boolean) TunableValues.getBoolean() + */ +public class TunableValues { + private static final String pathPrefix = "/Tunables/"; + + private static final HashMap doubleTunables = new HashMap<>(); + private static final HashMap intTunables = new HashMap<>(); + private static final HashMap boolTunables = new HashMap<>(); + + /** + * An object representing a tunable double value. + * + * @see TunableValues#getDouble(String, double) Creating a tunable double + */ + public static class TunableDouble implements Cloneable { + private final DoubleEntry entry; + private final double defaultValue; + + private TunableDouble(String path, double defaultValue) { + this.entry = NetworkTableInstance + .getDefault() + .getDoubleTopic(pathPrefix + path) + .getEntry(defaultValue); + entry.setDefault(defaultValue); + this.defaultValue = defaultValue; + } + + private TunableDouble(DoubleEntry entry, double defaultValue) { + this.entry = entry; + this.defaultValue = defaultValue; + } + + /** + * Get the value of the tunable, returns the default value a value is not present. + * + * @return The value of the tunable + */ + public double value() { + return entry.get(defaultValue); + } + + @Override + public TunableDouble clone() { + return new TunableDouble(entry, defaultValue); + } + } + + /** + * Get a tunable double value. + * + *

Tunable values are cached, so calling this method with the same path will return the same object + * and not create a new one (throwing out the new default value if it was different). + * + * @param path The path to the tunable value (it will be prefixed with "/Tunables/") + * @param defaultValue The default value of the tunable + * @return The tunable value object + * + * @see TunableDouble#value() How to get a value from the object + */ + public static TunableDouble getDouble(String path, double defaultValue) { + if (doubleTunables.containsKey(path)) { + return doubleTunables.get(path); + } else { + TunableDouble newTunable = new TunableDouble(path, defaultValue); + doubleTunables.put(path, newTunable); + return newTunable; + } + } + + /** + * An object representing a tunable integer value. + * + * @see TunableValues#getInteger(String, int) Creating a tunable integer + */ + public static class TunableInteger implements Cloneable { + private final IntegerEntry entry; + private final int defaultValue; + + private TunableInteger(String path, int defaultValue) { + this.entry = NetworkTableInstance + .getDefault() + .getIntegerTopic(pathPrefix + path) + .getEntry(defaultValue); + entry.setDefault(defaultValue); + this.defaultValue = defaultValue; + } + + private TunableInteger(IntegerEntry entry, int defaultValue) { + this.entry = entry; + this.defaultValue = defaultValue; + } + + /** + * Get the value of the tunable, returns the default value a value is not present. + * + * @return The value of the tunable + */ + public int value() { + return (int) Math.min(Math.max(entry.get(defaultValue), Integer.MIN_VALUE), Integer.MAX_VALUE); + } + + @Override + public TunableInteger clone() { + return new TunableInteger(entry, defaultValue); + } + } + + /** + * Get a tunable integer value. + * + *

Tunable values are cached, so calling this method with the same path will return the same object + * and not create a new one (throwing out the new default value if it was different). + * + * @param path The path to the tunable value (it will be prefixed with "/Tunables/") + * @param defaultValue The default value of the tunable + * @return The tunable value object + * + * @see TunableInteger#value() How to get a value from the object + */ + public static TunableInteger getInteger(String path, int defaultValue) { + if (intTunables.containsKey(path)) { + return intTunables.get(path); + } else { + TunableInteger newTunable = new TunableInteger(path, defaultValue); + intTunables.put(path, newTunable); + return newTunable; + } + } + + /** + * An object representing a tunable boolean value. + * + * @see TunableValues#getBoolean(String, boolean) Creating a tunable boolean + */ + public static class TunableBoolean implements Cloneable { + private final BooleanEntry entry; + private final boolean defaultValue; + + private TunableBoolean(String path, boolean defaultValue) { + this.entry = NetworkTableInstance + .getDefault() + .getBooleanTopic(pathPrefix + path) + .getEntry(defaultValue); + entry.setDefault(defaultValue); + this.defaultValue = defaultValue; + } + + private TunableBoolean(BooleanEntry entry, boolean defaultValue) { + this.entry = entry; + this.defaultValue = defaultValue; + } + + /** + * Get the value of the tunable, returns the default value if a value is not present. + * + * @return The value of the tunable + */ + public boolean value() { + return entry.get(defaultValue); + } + + @Override + public TunableBoolean clone() { + return new TunableBoolean(entry, defaultValue); + } + } + + /** + * Get a tunable boolean value. + * + *

Tunable values are cached, so calling this method with the same path will return the same object + * and not create a new one (throwing out the new default value if it was different). + * + * @param path The path to the tunable value (it will be prefixed with "/Tunables/") + * @param defaultValue The default value of the tunable + * @return The tunable value object + * + * @see TunableBoolean#value() How to get a value from the object + */ + public static TunableBoolean getBoolean(String path, boolean defaultValue) { + if (boolTunables.containsKey(path)) { + return boolTunables.get(path); + } else { + TunableBoolean newTunable = new TunableBoolean(path, defaultValue); + boolTunables.put(path, newTunable); + return newTunable; + } + } +} From 8975ed2e2b81f66535f75359ae908c6a9cf330d6 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Thu, 19 Sep 2024 14:09:25 -0400 Subject: [PATCH 4/6] Revert "rework tracer" This reverts commit 427d3c1e6554fa70b1d8dc641b92d8c5459eb313. --- .../wpi/first/wpilibj/IterativeRobotBase.java | 37 +- .../edu/wpi/first/wpilibj/TimedRobot.java | 4 - .../java/edu/wpi/first/wpilibj/Tracer.java | 328 ++++-------------- .../java/edu/wpi/first/wpilibj/Watchdog.java | 24 ++ .../edu/wpi/first/wpilibj/WatchdogTest.java | 30 ++ 5 files changed, 153 insertions(+), 270 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index d1207fc5ba1..b0c46604283 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -299,8 +299,6 @@ public double getPeriod() { /** Loop function. */ protected void loopFunc() { - Tracer.startTrace("RobotLoop"); - DriverStation.refreshData(); m_watchdog.reset(); @@ -346,12 +344,15 @@ protected void loopFunc() { switch (mode) { case kDisabled -> { disabledInit(); + m_watchdog.addEpoch("disabledInit()"); } case kAutonomous -> { autonomousInit(); + m_watchdog.addEpoch("autonomousInit()"); } case kTeleop -> { teleopInit(); + m_watchdog.addEpoch("teleopInit()"); } case kTest -> { if (m_lwEnabledInTest) { @@ -359,6 +360,7 @@ protected void loopFunc() { Shuffleboard.enableActuatorWidgets(); } testInit(); + m_watchdog.addEpoch("testInit()"); } default -> { // NOP @@ -372,35 +374,44 @@ protected void loopFunc() { switch (mode) { case kDisabled -> { DriverStationJNI.observeUserProgramDisabled(); - Tracer.traceFunc("DisabledPeriodic", this::disabledPeriodic); + disabledPeriodic(); + m_watchdog.addEpoch("disabledPeriodic()"); } case kAutonomous -> { DriverStationJNI.observeUserProgramAutonomous(); - Tracer.traceFunc("AutonomousPeriodic", this::autonomousPeriodic); + autonomousPeriodic(); + m_watchdog.addEpoch("autonomousPeriodic()"); } case kTeleop -> { DriverStationJNI.observeUserProgramTeleop(); - Tracer.traceFunc("TeleopPeriodic", this::teleopPeriodic); + teleopPeriodic(); + m_watchdog.addEpoch("teleopPeriodic()"); } case kTest -> { DriverStationJNI.observeUserProgramTest(); - Tracer.traceFunc("TestPeriodic", this::testPeriodic); + testPeriodic(); + m_watchdog.addEpoch("testPeriodic()"); } default -> { // NOP } } - Tracer.traceFunc("RobotPeriodic", this::robotPeriodic); + robotPeriodic(); + m_watchdog.addEpoch("robotPeriodic()"); SmartDashboard.updateValues(); + m_watchdog.addEpoch("SmartDashboard.updateValues()"); LiveWindow.updateValues(); + m_watchdog.addEpoch("LiveWindow.updateValues()"); Shuffleboard.update(); + m_watchdog.addEpoch("Shuffleboard.update()"); if (isSimulation()) { HAL.simPeriodicBefore(); - Tracer.traceFunc("SimulationPeriodic", this::simulationPeriodic); + simulationPeriodic(); HAL.simPeriodicAfter(); + m_watchdog.addEpoch("simulationPeriodic()"); } m_watchdog.disable(); @@ -410,7 +421,15 @@ protected void loopFunc() { NetworkTableInstance.getDefault().flushLocal(); } - Tracer.endTrace(); + // Warn on loop time overruns + if (m_watchdog.isExpired()) { + m_watchdog.printEpochs(); + } + } + + /** Prints list of epochs added so far and their times. */ + public void printWatchdogEpochs() { + m_watchdog.printEpochs(); } private void printLoopOverrunMessage() { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index f514dd67957..7b59f528a6b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -128,8 +128,6 @@ public void startCompetition() { break; } - Tracer.startTrace("TimedRobot"); - callback.func.run(); // Increment the expiration time by the number of full periods it's behind @@ -152,8 +150,6 @@ public void startCompetition() { + (currentTime - callback.expirationTime) / callback.period * callback.period; m_callbacks.add(callback); } - - Tracer.endTrace(); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java index 4723d2445b2..15ca7fff81f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java @@ -4,267 +4,81 @@ package edu.wpi.first.wpilibj; -import java.lang.management.GarbageCollectorMXBean; -import java.lang.management.ManagementFactory; -import java.util.ArrayList; import java.util.HashMap; -import java.util.function.Supplier; -import java.util.concurrent.atomic.AtomicBoolean; - -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; +import java.util.Map; +import java.util.function.Consumer; /** - * A Utility class for tracing code execution time. - * Will put info to NetworkTables under the "Tracer" table. - * - *

Example inside {@code Robot.java} - * 


- * @Override
- * public void robotPeriodic() {
- *   Tracer.traceFunc("CommandScheduler", scheduler::run);
- *   Tracer.traceFunc("MyVendorDep", MyVendorDep::updateAll);
- * }
- *
- * - *

Example inside a {@code Drive Subsystem} - * 


- * public void periodic() {
- *   Tracer.startTrace("DrivePeriodic");
- *   for (var module : modules) {
- *     Tracer.traceFunc("Module" + module.getName(), module::update);
- *   }
- *   Tracer.traceFunc("Gyro", gyro::update);
- *   Tracer.endTrace();
- * }
- *
+ * A class for keeping track of how much time it takes for different parts of code to execute. This + * is done with epochs, that are added by calls to {@link #addEpoch(String)}, and can be printed + * with a call to {@link #printEpochs()}. + * + *

Epochs are a way to partition the time elapsed so that when overruns occur, one can determine + * which parts of an operation consumed the most time. */ public class Tracer { - private static final class TraceStartData { - private double startTime = 0.0; - private double startGCTotalTime = 0.0; - - private void set(double startTime, double startGCTotalTime) { - this.startTime = startTime; - this.startGCTotalTime = startGCTotalTime; - } - } - - /** - * All of the tracers persistent state in a single object to be stored in a - * {@link ThreadLocal}. - */ - private static final class TracerState { - private final NetworkTable rootTable; - - // the stack of traces, every startTrace will add to this stack - // and every endTrace will remove from this stack - private final ArrayList traceStack = new ArrayList<>(); - // ideally we only need `traceStack` but in the interest of memory optimization - // and string concatenation speed we store the history of the stack to reuse the stack names - private final ArrayList traceStackHistory = new ArrayList<>(); - // the time of each trace, the key is the trace name, modified every endTrace - private final HashMap traceTimes = new HashMap<>(); - // the start time of each trace and the gc time at the start of the trace, - // the key is the trace name, modified every startTrace and endTrace. - private final HashMap traceStartTimes = new HashMap<>(); - // the publishers for each trace, the key is the trace name, modified every endCycle - private final HashMap publisherHeap = new HashMap<>(); - - // the garbage collector beans - private final ArrayList gcs = new ArrayList<>(ManagementFactory.getGarbageCollectorMXBeans()); - private final DoublePublisher gcTimeEntry; - private double gcTimeThisCycle = 0.0; - - private TracerState(String threadName) { - if (threadName == null) { - this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer"); - } else { - this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer").getSubTable(threadName); - } - this.gcTimeEntry = rootTable.getDoubleTopic("GCTime").publishEx( - "double", - "{ \"cached\": false}" - ); - } - - private String appendTraceStack(String trace) { - traceStack.add(trace); - StringBuilder sb = new StringBuilder(); - for (int i = 0; i < traceStack.size(); i++) { - sb.append(traceStack.get(i)); - if (i < traceStack.size() - 1) { - sb.append("/"); - } - } - String str = sb.toString(); - traceStackHistory.add(str); - return str; - } - - private String popTraceStack() { - traceStack.remove(traceStack.size() - 1); - return traceStackHistory.remove(traceStackHistory.size() - 1); - } - - private double totalGCTime() { - double gcTime = 0; - for (GarbageCollectorMXBean gc : gcs) { - gcTime += gc.getCollectionTime(); - } - return gcTime; - } - - private void endCycle() { - // update times for all already existing publishers - for (var publisher : publisherHeap.entrySet()) { - Double time = traceTimes.remove(publisher.getKey()); - if (time == null) time = 0.0; - publisher.getValue().set(time); - } - // create publishers for all new entries - for (var traceTime : traceTimes.entrySet()) { - DoublePublisher publisher = rootTable.getDoubleTopic(traceTime.getKey()).publishEx( - "double", - "{ \"cached\": false}" - ); - publisher.set(traceTime.getValue()); - publisherHeap.put(traceTime.getKey(), publisher); - } - - // log gc time - if (gcs.size() > 0) gcTimeEntry.set(gcTimeThisCycle); - gcTimeThisCycle = 0.0; - - // clean up state - traceTimes.clear(); - traceStackHistory.clear(); - } - } - - private static final AtomicBoolean singleThreadedMode = new AtomicBoolean(false); - private static final ThreadLocal threadLocalState = ThreadLocal.withInitial(() -> { - if (singleThreadedMode.get()) { - throw new IllegalStateException("Single threaded mode is enabled, cannot create new TracerState"); - } - return new TracerState(Thread.currentThread().getName()); - }); - - private static void startTraceInner(final String name, final TracerState state) { - String stack = state.appendTraceStack(name); - TraceStartData data = state.traceStartTimes.get(stack); - if (data == null) { - data = new TraceStartData(); - state.traceStartTimes.put(stack, data); - } - data.set(Timer.getFPGATimestamp() * 1_000.0, state.totalGCTime()); - } - - private static void endTraceInner(final TracerState state) { - try { - String stack = state.popTraceStack(); - var startData = state.traceStartTimes.get(stack); - double gcTimeSinceStart = state.totalGCTime() - startData.startGCTotalTime; - state.gcTimeThisCycle += gcTimeSinceStart; - state.traceTimes.put( - stack, - Timer.getFPGATimestamp() * 1_000.0 - - startData.startTime - - gcTimeSinceStart - ); - if (state.traceStack.size() == 0) { - state.endCycle(); - } - } catch (Exception e) { - DriverStation.reportError("[Tracer] An end trace was called with no opening trace " + e, true); - for (var trace : state.traceStack) { - DriverStation.reportError("[Tracer] Open trace: " + trace, false); - } - state.endCycle(); - } - } - - /** - * Starts a trace, - * should be called at the beginning of a function thats not being called by - * user code. - * Should be paired with {@link Tracer#endTrace()} at the end of the function. - * - * Best used in periodic functions in Subsystems and Robot.java. - * - * @param name the name of the trace, should be unique to the function. - */ - public static void startTrace(String name) { - startTraceInner(name, threadLocalState.get()); - } - - /** - * Ends a trace, should only be called at the end of a function thats not being - * called by user code. - * If a {@link Tracer#startTrace(String)} is not paired with a - * {@link Tracer#endTrace()} there could be a crash. - */ - public static void endTrace() { - endTraceInner(threadLocalState.get()); - } - - /** - * Disables garbage collection logging for the current thread. - * This can help performance in some cases. - */ - public static void disableGcLoggingForCurrentThread() { - TracerState state = threadLocalState.get(); - state.gcTimeEntry.close(); - state.gcs.clear(); - } - - /** - * Enables single threaded mode for the Tracer. - * This will cause traces on different threads to throw an exception. - * This will shorten the path of traced data in NetworkTables by not including the thread name. - * - *

Warning: This will throw an exception if called after any traces have been started. - */ - public static void enableSingleThreadedMode() { - threadLocalState.set(new TracerState(null)); - singleThreadedMode.set(true); - } - - /** - * Traces a function, should be used in place of - * {@link Tracer#startTrace(String)} and {@link Tracer#endTrace()} - * for functions called by user code like {@code CommandScheduler.run()} and - * other expensive functions. - * - * @param name the name of the trace, should be unique to the function. - * @param runnable the function to trace. - * - * @apiNote If you want to return a value then use - * {@link Tracer#traceFunc(String, Supplier)}. - */ - public static void traceFunc(String name, Runnable runnable) { - final TracerState state = threadLocalState.get(); - startTraceInner(name, state); - runnable.run(); - endTraceInner(state); - } - - /** - * Traces a function, should be used in place of - * {@link Tracer#startTrace(String)} and {@link Tracer#endTrace()} - * for functions called by user code like {@code CommandScheduler.run()} and - * other expensive functions. - * - * @param name the name of the trace, should be unique to the function. - * @param supplier the function to trace. - */ - public static R traceFunc(String name, Supplier supplier) { - final TracerState state = threadLocalState.get(); - startTraceInner(name, state); - R ret = supplier.get(); - endTraceInner(state); - return ret; + private static final long kMinPrintPeriod = 1000000; // microseconds + + private long m_lastEpochsPrintTime; // microseconds + private long m_startTime; // microseconds + + private final Map m_epochs = new HashMap<>(); // microseconds + + /** Tracer constructor. */ + public Tracer() { + resetTimer(); + } + + /** Clears all epochs. */ + public void clearEpochs() { + m_epochs.clear(); + resetTimer(); + } + + /** Restarts the epoch timer. */ + public final void resetTimer() { + m_startTime = RobotController.getFPGATime(); + } + + /** + * Adds time since last epoch to the list printed by printEpochs(). + * + *

Epochs are a way to partition the time elapsed so that when overruns occur, one can + * determine which parts of an operation consumed the most time. + * + *

This should be called immediately after execution has finished, with a call to this method + * or {@link #resetTimer()} before execution. + * + * @param epochName The name to associate with the epoch. + */ + public void addEpoch(String epochName) { + long currentTime = RobotController.getFPGATime(); + m_epochs.put(epochName, currentTime - m_startTime); + m_startTime = currentTime; + } + + /** Prints list of epochs added so far and their times to the DriverStation. */ + public void printEpochs() { + printEpochs(out -> DriverStation.reportWarning(out, false)); + } + + /** + * Prints list of epochs added so far and their times to the entered String consumer. + * + *

This overload can be useful for logging to a file, etc. + * + * @param output the stream that the output is sent to + */ + public void printEpochs(Consumer output) { + long now = RobotController.getFPGATime(); + if (now - m_lastEpochsPrintTime > kMinPrintPeriod) { + StringBuilder sb = new StringBuilder(); + m_lastEpochsPrintTime = now; + m_epochs.forEach( + (key, value) -> sb.append(String.format("\t%s: %.6fs\n", key, value / 1.0e6))); + if (sb.length() > 0) { + output.accept(sb.toString()); + } } + } } - diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java index ad3c49d3446..6d00d6402bb 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -31,6 +31,8 @@ public class Watchdog implements Closeable, Comparable { boolean m_suppressTimeoutMessage; + private final Tracer m_tracer; + private static final PriorityQueue m_watchdogs = new PriorityQueue<>(); private static ReentrantLock m_queueMutex = new ReentrantLock(); private static int m_notifier; @@ -50,6 +52,7 @@ public class Watchdog implements Closeable, Comparable { public Watchdog(double timeoutSeconds, Runnable callback) { m_timeoutSeconds = timeoutSeconds; m_callback = callback; + m_tracer = new Tracer(); } @Override @@ -91,6 +94,7 @@ public double getTime() { */ public void setTimeout(double timeoutSeconds) { m_startTimeSeconds = Timer.getFPGATimestamp(); + m_tracer.clearEpochs(); m_queueMutex.lock(); try { @@ -134,6 +138,25 @@ public boolean isExpired() { } } + /** + * Adds time since last epoch to the list printed by printEpochs(). + * + * @see Tracer#addEpoch(String) + * @param epochName The name to associate with the epoch. + */ + public void addEpoch(String epochName) { + m_tracer.addEpoch(epochName); + } + + /** + * Prints list of epochs added so far and their times. + * + * @see Tracer#printEpochs() + */ + public void printEpochs() { + m_tracer.printEpochs(); + } + /** * Resets the watchdog timer. * @@ -146,6 +169,7 @@ public void reset() { /** Enables the watchdog timer. */ public void enable() { m_startTimeSeconds = Timer.getFPGATimestamp(); + m_tracer.clearEpochs(); m_queueMutex.lock(); try { diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java index 4356903a340..5e9e1229418 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java @@ -117,6 +117,36 @@ void isExpiredTest() { } } + @Test + @ResourceLock("timing") + void epochsTest() { + final AtomicInteger watchdogCounter = new AtomicInteger(0); + + try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) { + // Run 1 + watchdog.enable(); + watchdog.addEpoch("Epoch 1"); + SimHooks.stepTiming(0.1); + watchdog.addEpoch("Epoch 2"); + SimHooks.stepTiming(0.1); + watchdog.addEpoch("Epoch 3"); + watchdog.disable(); + + assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); + + // Run 2 + watchdog.enable(); + watchdog.addEpoch("Epoch 1"); + SimHooks.stepTiming(0.2); + watchdog.reset(); + SimHooks.stepTiming(0.2); + watchdog.addEpoch("Epoch 2"); + watchdog.disable(); + + assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); + } + } + @Test @ResourceLock("timing") void multiWatchdogTest() { From 7035c612fdc83e0e6387a198cc31c5b24a83e475 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Thu, 19 Sep 2024 14:09:28 -0400 Subject: [PATCH 5/6] Revert "fixed CommandScheduler for now" This reverts commit 2294b0b8b63e4770a34ef5f2418ebf1ef0ff57d5. --- .../first/wpilibj2/command/CommandScheduler.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 2e924a66843..7e634cf5c92 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -170,6 +170,8 @@ private void initCommand(Command command, Set requirements) { for (Consumer action : m_initActions) { action.accept(command); } + + m_watchdog.addEpoch(command.getName() + ".initialize()"); } /** @@ -262,6 +264,7 @@ public void run() { if (RobotBase.isSimulation()) { subsystem.simulationPeriodic(); } + m_watchdog.addEpoch(subsystem.getName() + ".periodic()"); } // Cache the active instance to avoid concurrency problems if setActiveLoop() is called from @@ -269,6 +272,7 @@ public void run() { EventLoop loopCache = m_activeButtonLoop; // Poll buttons for new commands to add. loopCache.poll(); + m_watchdog.addEpoch("buttons.run()"); m_inRunLoop = true; boolean isDisabled = RobotState.isDisabled(); @@ -285,6 +289,7 @@ public void run() { for (Consumer action : m_executeActions) { action.accept(command); } + m_watchdog.addEpoch(command.getName() + ".execute()"); if (command.isFinished()) { m_endingCommands.add(command); command.end(false); @@ -295,6 +300,7 @@ public void run() { iterator.remove(); m_requirements.keySet().removeAll(command.getRequirements()); + m_watchdog.addEpoch(command.getName() + ".end(false)"); } } m_inRunLoop = false; @@ -323,6 +329,7 @@ public void run() { m_watchdog.disable(); if (m_watchdog.isExpired()) { System.out.println("CommandScheduler loop overrun"); + m_watchdog.printEpochs(); } } @@ -480,6 +487,7 @@ private void cancel(Command command, Optional interruptor) { m_endingCommands.remove(command); m_scheduledCommands.remove(command); m_requirements.keySet().removeAll(command.getRequirements()); + m_watchdog.addEpoch(command.getName() + ".end(true)"); } /** Cancels all commands that are currently scheduled. */ @@ -522,6 +530,11 @@ public void enable() { m_disabled = false; } + /** Prints list of epochs added so far and their times. */ + public void printWatchdogEpochs() { + m_watchdog.printEpochs(); + } + /** * Adds an action to perform on the initialization of any command by the scheduler. * From 89faad64215068fb47e85e5051cf4bc5d695ee07 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 22 Sep 2024 21:46:08 -0400 Subject: [PATCH 6/6] ran fmt --- .../edu/wpi/first/wpilibj/TunableValues.java | 36 ++++++++++--------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TunableValues.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TunableValues.java index 733d9c57030..0b364756954 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TunableValues.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TunableValues.java @@ -1,3 +1,7 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package edu.wpi.first.wpilibj; import java.util.HashMap; @@ -10,7 +14,7 @@ /** * An api for creating NT tunable values. * Bools, Ints and Doubles are supported. - * + * * @see TunableValues#getDouble(String, double) TunableValues.getDouble() * @see TunableValues#getInteger(String, int) TunableValues.getInteger() * @see TunableValues#getBoolean(String, boolean) TunableValues.getBoolean() @@ -24,7 +28,7 @@ public class TunableValues { /** * An object representing a tunable double value. - * + * * @see TunableValues#getDouble(String, double) Creating a tunable double */ public static class TunableDouble implements Cloneable { @@ -47,7 +51,7 @@ private TunableDouble(DoubleEntry entry, double defaultValue) { /** * Get the value of the tunable, returns the default value a value is not present. - * + * * @return The value of the tunable */ public double value() { @@ -62,14 +66,14 @@ public TunableDouble clone() { /** * Get a tunable double value. - * + * *

Tunable values are cached, so calling this method with the same path will return the same object * and not create a new one (throwing out the new default value if it was different). - * + * * @param path The path to the tunable value (it will be prefixed with "/Tunables/") * @param defaultValue The default value of the tunable * @return The tunable value object - * + * * @see TunableDouble#value() How to get a value from the object */ public static TunableDouble getDouble(String path, double defaultValue) { @@ -84,7 +88,7 @@ public static TunableDouble getDouble(String path, double defaultValue) { /** * An object representing a tunable integer value. - * + * * @see TunableValues#getInteger(String, int) Creating a tunable integer */ public static class TunableInteger implements Cloneable { @@ -107,7 +111,7 @@ private TunableInteger(IntegerEntry entry, int defaultValue) { /** * Get the value of the tunable, returns the default value a value is not present. - * + * * @return The value of the tunable */ public int value() { @@ -122,14 +126,14 @@ public TunableInteger clone() { /** * Get a tunable integer value. - * + * *

Tunable values are cached, so calling this method with the same path will return the same object * and not create a new one (throwing out the new default value if it was different). - * + * * @param path The path to the tunable value (it will be prefixed with "/Tunables/") * @param defaultValue The default value of the tunable * @return The tunable value object - * + * * @see TunableInteger#value() How to get a value from the object */ public static TunableInteger getInteger(String path, int defaultValue) { @@ -144,7 +148,7 @@ public static TunableInteger getInteger(String path, int defaultValue) { /** * An object representing a tunable boolean value. - * + * * @see TunableValues#getBoolean(String, boolean) Creating a tunable boolean */ public static class TunableBoolean implements Cloneable { @@ -167,7 +171,7 @@ private TunableBoolean(BooleanEntry entry, boolean defaultValue) { /** * Get the value of the tunable, returns the default value if a value is not present. - * + * * @return The value of the tunable */ public boolean value() { @@ -182,14 +186,14 @@ public TunableBoolean clone() { /** * Get a tunable boolean value. - * + * *

Tunable values are cached, so calling this method with the same path will return the same object * and not create a new one (throwing out the new default value if it was different). - * + * * @param path The path to the tunable value (it will be prefixed with "/Tunables/") * @param defaultValue The default value of the tunable * @return The tunable value object - * + * * @see TunableBoolean#value() How to get a value from the object */ public static TunableBoolean getBoolean(String path, boolean defaultValue) {