Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

SCRIPT Move java files

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 19:55:40 -05:00
committed by Peter Johnson
parent 7ca1be9bae
commit c350c5f112
1486 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

453
wpilibj/src/main/java/org/wpilib/system/DataLogManager.java Normal file
View File

@@ -0,0 +1,453 @@
// 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 edu.wpi.first.datalog.DataLog;
import edu.wpi.first.datalog.DataLogBackgroundWriter;
import edu.wpi.first.datalog.FileLogger;
import edu.wpi.first.datalog.IntegerLogEntry;
import edu.wpi.first.datalog.StringLogEntry;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.util.concurrent.Event;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.time.LocalDateTime;
import java.time.ZoneId;
import java.time.format.DateTimeFormatter;
import java.util.Arrays;
import java.util.Comparator;
import java.util.Random;
/**
* Centralized data log that provides automatic data log file management. It automatically cleans up
* old files when disk space is low and renames the file based either on current date/time or (if
* available) competition match number. The data file will be saved to a USB flash drive in a folder
* named "logs" if one is attached, or to /home/systemcore/logs otherwise.
*
* <p>Log files are initially named "FRC_TBD_{random}.wpilog" until the DS connects. After the DS
* connects, the log file is renamed to "FRC_yyyyMMdd_HHmmss.wpilog" (where the date/time is UTC).
* If the FMS is connected and provides a match number, the log file is renamed to
* "FRC_yyyyMMdd_HHmmss_{event}_{match}.wpilog".
*
* <p>On startup, all existing FRC_TBD log files are deleted. If there is less than 50 MB of free
* space on the target storage, FRC_ log files are deleted (oldest to newest) until there is 50 MB
* free OR there are 10 files remaining.
*
* <p>By default, all NetworkTables value changes are stored to the data log.
*/
public final class DataLogManager {
private static DataLogBackgroundWriter m_log;
private static boolean m_stopped;
private static String m_logDir;
private static boolean m_filenameOverride;
private static Thread m_thread;
private static final ZoneId m_utc = ZoneId.of("UTC");
private static final DateTimeFormatter m_timeFormatter =
DateTimeFormatter.ofPattern("yyyyMMdd_HHmmss").withZone(m_utc);
private static boolean m_ntLoggerEnabled = true;
private static int m_ntEntryLogger;
private static int m_ntConnLogger;
private static boolean m_consoleLoggerEnabled = true;
private static FileLogger m_consoleLogger;
private static StringLogEntry m_messageLog;
// if less than this much free space, delete log files until there is this much free space
// OR there are this many files remaining.
private static final long kFreeSpaceThreshold = 50000000L;
private static final int kFileCountThreshold = 10;
private DataLogManager() {}
/** Start data log manager with default directory location. */
public static synchronized void start() {
start("", "", 0.25);
}
/**
* Start data log manager. The parameters have no effect if the data log manager was already
* started (e.g. by calling another static function).
*
* @param dir if not empty, directory to use for data log storage
*/
public static synchronized void start(String dir) {
start(dir, "", 0.25);
}
/**
* Start data log manager. The parameters have no effect if the data log manager was already
* started (e.g. by calling another static function).
*
* @param dir if not empty, directory to use for data log storage
* @param filename filename to use; if none provided, the filename is automatically generated
*/
public static synchronized void start(String dir, String filename) {
start(dir, filename, 0.25);
}
/**
* Start data log manager. The parameters have no effect if the data log manager was already
* started (e.g. by calling another static function).
*
* @param dir if not empty, directory to use for data log storage
* @param filename filename to use; if none provided, the filename is automatically generated
* @param period time between automatic flushes to disk, in seconds; this is a time/storage
* tradeoff
*/
public static synchronized void start(String dir, String filename, double period) {
if (m_log == null) {
m_logDir = makeLogDir(dir);
m_filenameOverride = !filename.isEmpty();
// Delete all previously existing FRC_TBD_*.wpilog files. These only exist when the robot
// never connects to the DS, so they are very unlikely to have useful data and just clutter
// the filesystem.
File[] files =
new File(m_logDir)
.listFiles((d, name) -> name.startsWith("FRC_TBD_") && name.endsWith(".wpilog"));
if (files != null) {
for (File file : files) {
if (!file.delete()) {
System.err.println("DataLogManager: could not delete " + file);
}
}
}
m_log = new DataLogBackgroundWriter(m_logDir, makeLogFilename(filename), period);
m_messageLog = new StringLogEntry(m_log, "messages");
// Log all NT entries and connections
if (m_ntLoggerEnabled) {
startNtLog();
}
// Log console output
if (m_consoleLoggerEnabled) {
startConsoleLog();
}
} else if (m_stopped) {
m_log.setFilename(makeLogFilename(filename));
m_log.resume();
m_stopped = false;
}
if (m_thread == null) {
m_thread = new Thread(DataLogManager::logMain, "DataLogDS");
m_thread.setDaemon(true);
m_thread.start();
}
}
/** Stop data log manager. */
public static synchronized void stop() {
if (m_thread != null) {
m_thread.interrupt();
m_thread = null;
}
if (m_log != null) {
m_log.stop();
m_stopped = true;
}
}
/**
* Log a message to the "messages" entry. The message is also printed to standard output (followed
* by a newline).
*
* @param message message
*/
public static synchronized void log(String message) {
if (m_messageLog == null) {
start();
}
m_messageLog.append(message);
System.out.println(message);
}
/**
* Get the managed data log (for custom logging). Starts the data log manager if not already
* started.
*
* @return data log
*/
public static synchronized DataLog getLog() {
if (m_log == null) {
start();
}
return m_log;
}
/**
* Get the log directory.
*
* @return log directory, or empty string if logging not yet started
*/
public static synchronized String getLogDir() {
if (m_logDir == null) {
return "";
}
return m_logDir;
}
/**
* Enable or disable logging of NetworkTables data. Note that unlike the network interface for
* NetworkTables, this will capture every value change. Defaults to enabled.
*
* @param enabled true to enable, false to disable
*/
public static synchronized void logNetworkTables(boolean enabled) {
boolean wasEnabled = m_ntLoggerEnabled;
m_ntLoggerEnabled = enabled;
if (m_log == null) {
start();
return;
}
if (enabled && !wasEnabled) {
startNtLog();
} else if (!enabled && wasEnabled) {
stopNtLog();
}
}
/**
* Enable or disable logging of the console output. Defaults to enabled.
*
* @param enabled true to enable, false to disable
*/
public static synchronized void logConsoleOutput(boolean enabled) {
boolean wasEnabled = m_consoleLoggerEnabled;
m_consoleLoggerEnabled = enabled;
if (m_log == null) {
start();
return;
}
if (enabled && !wasEnabled) {
startConsoleLog();
} else if (!enabled && wasEnabled) {
stopConsoleLog();
}
}
private static String makeLogDir(String dir) {
if (!dir.isEmpty()) {
return dir;
}
if (RobotBase.isReal()) {
try {
// prefer a mounted USB drive if one is accessible
Path usbDir = Paths.get("/u").toRealPath();
if (Files.isWritable(usbDir)) {
if (!new File("/u/logs").mkdir()) {
// ignored
}
HAL.reportUsage("DataLogManager", "USB");
return "/u/logs";
}
} catch (IOException ex) {
// ignored
}
if (!new File("/home/systemcore/logs").mkdir()) {
// ignored
}
HAL.reportUsage("DataLogManager", "Onboard");
return "/home/systemcore/logs";
}
String logDir = Filesystem.getOperatingDirectory().getAbsolutePath() + "/logs";
if (!new File(logDir).mkdir()) {
// ignored
}
return logDir;
}
private static String makeLogFilename(String filenameOverride) {
if (!filenameOverride.isEmpty()) {
return filenameOverride;
}
Random rnd = new Random();
StringBuilder filename = new StringBuilder();
filename.append("FRC_TBD_");
for (int i = 0; i < 4; i++) {
filename.append(String.format("%04x", rnd.nextInt(0x10000)));
}
filename.append(".wpilog");
return filename.toString();
}
private static void startNtLog() {
NetworkTableInstance inst = NetworkTableInstance.getDefault();
m_ntEntryLogger = inst.startEntryDataLog(m_log, "", "NT:");
m_ntConnLogger = inst.startConnectionDataLog(m_log, "NTConnection");
}
private static void stopNtLog() {
NetworkTableInstance.stopEntryDataLog(m_ntEntryLogger);
NetworkTableInstance.stopConnectionDataLog(m_ntConnLogger);
}
private static void startConsoleLog() {
if (RobotBase.isReal()) {
m_consoleLogger = new FileLogger("/home/systemcore/FRC_UserProgram.log", m_log, "console");
}
}
private static void stopConsoleLog() {
if (RobotBase.isReal()) {
m_consoleLogger.close();
}
}
private static void logMain() {
// based on free disk space, scan for "old" FRC_*.wpilog files and remove
{
File logDir = new File(m_logDir);
long freeSpace = logDir.getUsableSpace();
if (freeSpace < kFreeSpaceThreshold) {
// Delete oldest FRC_*.wpilog files (ignore FRC_TBD_*.wpilog as we just created one)
File[] files =
logDir.listFiles(
(dir, name) ->
name.startsWith("FRC_")
&& name.endsWith(".wpilog")
&& !name.startsWith("FRC_TBD_"));
if (files != null) {
Arrays.sort(files, Comparator.comparingLong(File::lastModified));
int count = files.length;
for (File file : files) {
--count;
if (count < kFileCountThreshold) {
break;
}
long length = file.length();
if (file.delete()) {
DriverStation.reportWarning("DataLogManager: Deleted " + file.getName(), false);
freeSpace += length;
if (freeSpace >= kFreeSpaceThreshold) {
break;
}
} else {
System.err.println("DataLogManager: could not delete " + file);
}
}
}
} else if (freeSpace < 2 * kFreeSpaceThreshold) {
DriverStation.reportWarning(
"DataLogManager: Log storage device has "
+ freeSpace / 1000000
+ " MB of free space remaining! Logs will get deleted below "
+ kFreeSpaceThreshold / 1000000
+ " MB of free space. "
+ "Consider deleting logs off the storage device.",
false);
}
}
int timeoutCount = 0;
boolean paused = false;
int dsAttachCount = 0;
int fmsAttachCount = 0;
boolean dsRenamed = m_filenameOverride;
boolean fmsRenamed = m_filenameOverride;
int sysTimeCount = 0;
IntegerLogEntry sysTimeEntry =
new IntegerLogEntry(
m_log, "systemTime", "{\"source\":\"DataLogManager\",\"format\":\"time_t_us\"}");
Event newDataEvent = new Event();
DriverStation.provideRefreshedDataEventHandle(newDataEvent.getHandle());
while (!Thread.interrupted()) {
boolean timedOut;
try {
timedOut = WPIUtilJNI.waitForObjectTimeout(newDataEvent.getHandle(), 0.25);
} catch (InterruptedException e) {
break;
}
if (Thread.interrupted()) {
break;
}
if (timedOut) {
timeoutCount++;
// pause logging after being disconnected for 10 seconds
if (timeoutCount > 40 && !paused) {
timeoutCount = 0;
paused = true;
m_log.pause();
}
continue;
}
// when we connect to the DS, resume logging
timeoutCount = 0;
if (paused) {
paused = false;
m_log.resume();
}
if (!dsRenamed) {
// track DS attach
if (DriverStation.isDSAttached()) {
dsAttachCount++;
} else {
dsAttachCount = 0;
}
if (dsAttachCount > 50) { // 1 second
if (RobotController.isSystemTimeValid()) {
LocalDateTime now = LocalDateTime.now(m_utc);
m_log.setFilename("FRC_" + m_timeFormatter.format(now) + ".wpilog");
dsRenamed = true;
} else {
dsAttachCount = 0; // wait a bit and try again
}
}
}
if (!fmsRenamed) {
// track FMS attach
if (DriverStation.isFMSAttached()) {
fmsAttachCount++;
} else {
fmsAttachCount = 0;
}
if (fmsAttachCount > 250) { // 5 seconds
// match info comes through TCP, so we need to double-check we've
// actually received it
DriverStation.MatchType matchType = DriverStation.getMatchType();
if (matchType != DriverStation.MatchType.None) {
// rename per match info
char matchTypeChar =
switch (matchType) {
case Practice -> 'P';
case Qualification -> 'Q';
case Elimination -> 'E';
default -> '_';
};
m_log.setFilename(
"FRC_"
+ m_timeFormatter.format(LocalDateTime.now(m_utc))
+ "_"
+ DriverStation.getEventName()
+ "_"
+ matchTypeChar
+ DriverStation.getMatchNumber()
+ ".wpilog");
fmsRenamed = true;
dsRenamed = true; // don't override FMS rename
}
}
}
// Write system time every ~5 seconds
sysTimeCount++;
if (sysTimeCount >= 250) {
sysTimeCount = 0;
if (RobotController.isSystemTimeValid()) {
sysTimeEntry.append(WPIUtilJNI.getSystemTime(), WPIUtilJNI.now());
}
}
}
newDataEvent.close();
}
}

64
wpilibj/src/main/java/org/wpilib/system/Filesystem.java Normal file
View File

@@ -0,0 +1,64 @@
// 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.io.File;
/**
* Class for interacting with the Filesystem, particularly, interacting with FRC-related paths on
* the system, such as the launch and deploy directories.
*
* <p>This class is primarily used for obtaining resources in src/main/deploy, and the systemcore
* path /home/systemcore in a simulation-compatible way.
*/
public final class Filesystem {
private Filesystem() {}
/**
* Obtains the current working path that the program was launched with. This is analogous to the
* `pwd` command on unix.
*
* @return The current working directory (launch directory)
*/
public static File getLaunchDirectory() {
// workaround for
// https://www.chiefdelphi.com/t/filesystem-getdeploydirectory-returning-wrong-location-how-to-fix/427292
String path =
System.getProperty("user.dir")
.replace(
File.separator + "build" + File.separator + "jni" + File.separator + "release", "");
return new File(path).getAbsoluteFile();
}
/**
* Obtains the operating directory of the program. On the systemcore, this is /home/systemcore. In
* simulation, it is where the simulation was launched from (`pwd`).
*
* @return The operating directory
*/
public static File getOperatingDirectory() {
if (!RobotBase.isSimulation()) {
return new File("/home/systemcore");
} else {
return getLaunchDirectory();
}
}
/**
* Obtains the 'deploy' directory of the program, located at src/main/deploy, which is deployed by
* default. On the systemcore, this is /home/systemcore/deploy. In simulation, it is where the
* simulation was launched from, in the subdirectory "src/main/deploy" (`pwd`/src/main/deploy).
*
* @return The 'deploy' directory
*/
public static File getDeployDirectory() {
if (!RobotBase.isSimulation()) {
return new File(getOperatingDirectory(), "deploy");
} else {
return new File(
getOperatingDirectory(), "src" + File.separator + "main" + File.separator + "deploy");
}
}
}

283
wpilibj/src/main/java/org/wpilib/system/Notifier.java Normal file
View File

@@ -0,0 +1,283 @@
// 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 static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.hal.NotifierJNI;
import edu.wpi.first.units.measure.Frequency;
import edu.wpi.first.units.measure.Time;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.locks.ReentrantLock;
/**
* Notifiers run a user-provided callback function on a separate thread.
*
* <p>If startSingle() is used, the callback will run once. If startPeriodic() is used, the callback
* will run repeatedly with the given period until stop() is called.
*/
public class Notifier implements AutoCloseable {
// The thread waiting on the HAL alarm.
private Thread m_thread;
// The lock held while updating process information.
private final ReentrantLock m_processLock = new ReentrantLock();
// HAL handle passed to the JNI bindings (atomic for proper destruction).
private final AtomicInteger m_notifier = new AtomicInteger();
// The user-provided callback.
private Runnable m_callback;
// The time, in seconds, at which the callback should be called. Has the same
// zero as RobotController.getFPGATime().
private double m_expirationTime;
// If periodic, stores the callback period in seconds; if single, stores the time until
// the callback call in seconds.
private double m_period;
// True if the callback is periodic
private boolean m_periodic;
@Override
public void close() {
int handle = m_notifier.getAndSet(0);
if (handle == 0) {
return;
}
NotifierJNI.stopNotifier(handle);
// Join the thread to ensure the callback has exited.
if (m_thread.isAlive()) {
try {
m_thread.interrupt();
m_thread.join();
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
}
NotifierJNI.cleanNotifier(handle);
m_thread = null;
}
/**
* Update the alarm hardware to reflect the next alarm.
*
* @param triggerTimeMicroS the time in microseconds at which the next alarm will be triggered
*/
private void updateAlarm(long triggerTimeMicroS) {
int notifier = m_notifier.get();
if (notifier == 0) {
return;
}
NotifierJNI.updateNotifierAlarm(notifier, triggerTimeMicroS);
}
/** Update the alarm hardware to reflect the next alarm. */
private void updateAlarm() {
updateAlarm((long) (m_expirationTime * 1e6));
}
/**
* Create a Notifier with the given callback.
*
* <p>Configure when the callback runs with startSingle() or startPeriodic().
*
* @param callback The callback to run.
*/
public Notifier(Runnable callback) {
requireNonNullParam(callback, "callback", "Notifier");
m_callback = callback;
m_notifier.set(NotifierJNI.initializeNotifier());
m_thread =
new Thread(
() -> {
while (!Thread.interrupted()) {
int notifier = m_notifier.get();
if (notifier == 0) {
break;
}
long curTime = NotifierJNI.waitForNotifierAlarm(notifier);
if (curTime == 0) {
break;
}
Runnable threadHandler;
m_processLock.lock();
try {
threadHandler = m_callback;
if (m_periodic) {
m_expirationTime += m_period;
updateAlarm();
} else {
// Need to update the alarm to cause it to wait again
updateAlarm(-1);
}
} finally {
m_processLock.unlock();
}
// Call callback
if (threadHandler != null) {
threadHandler.run();
}
}
});
m_thread.setName("Notifier");
m_thread.setDaemon(true);
m_thread.setUncaughtExceptionHandler(
(thread, error) -> {
Throwable cause = error.getCause();
if (cause != null) {
error = cause;
}
DriverStation.reportError(
"Unhandled exception in Notifier thread: " + error, error.getStackTrace());
DriverStation.reportError(
"The Runnable for this Notifier (or methods called by it) should have handled "
+ "the exception above.\n"
+ " The above stacktrace can help determine where the error occurred.\n"
+ " See https://wpilib.org/stacktrace for more information.",
false);
});
m_thread.start();
}
/**
* Sets the name of the notifier. Used for debugging purposes only.
*
* @param name Name
*/
public void setName(String name) {
m_thread.setName(name);
NotifierJNI.setNotifierName(m_notifier.get(), name);
}
/**
* Change the callback function.
*
* @param callback The callback function.
*/
public void setCallback(Runnable callback) {
m_processLock.lock();
try {
m_callback = callback;
} finally {
m_processLock.unlock();
}
}
/**
* Run the callback once after the given delay.
*
* @param delay Time in seconds to wait before the callback is called.
*/
public void startSingle(double delay) {
m_processLock.lock();
try {
m_periodic = false;
m_period = delay;
m_expirationTime = RobotController.getFPGATime() * 1e-6 + delay;
updateAlarm();
} finally {
m_processLock.unlock();
}
}
/**
* Run the callback once after the given delay.
*
* @param delay Time to wait before the callback is called.
*/
public void startSingle(Time delay) {
startSingle(delay.in(Seconds));
}
/**
* Run the callback periodically with the given period.
*
* <p>The user-provided callback should be written so that it completes before the next time it's
* scheduled to run.
*
* @param period Period in seconds after which to call the callback starting one period after the
* call to this method.
*/
public void startPeriodic(double period) {
m_processLock.lock();
try {
m_periodic = true;
m_period = period;
m_expirationTime = RobotController.getFPGATime() * 1e-6 + period;
updateAlarm();
} finally {
m_processLock.unlock();
}
}
/**
* Run the callback periodically with the given period.
*
* <p>The user-provided callback should be written so that it completes before the next time it's
* scheduled to run.
*
* @param period Period after which to call the callback starting one period after the call to
* this method.
*/
public void startPeriodic(Time period) {
startPeriodic(period.in(Seconds));
}
/**
* Run the callback periodically with the given frequency.
*
* <p>The user-provided callback should be written so that it completes before the next time it's
* scheduled to run.
*
* @param frequency Frequency at which to call the callback, starting one period after the call to
* this method.
*/
public void startPeriodic(Frequency frequency) {
startPeriodic(frequency.asPeriod());
}
/**
* Stop further callback invocations.
*
* <p>No further periodic callbacks will occur. Single invocations will also be cancelled if they
* haven't yet occurred.
*
* <p>If a callback invocation is in progress, this function will block until the callback is
* complete.
*/
public void stop() {
m_processLock.lock();
try {
m_periodic = false;
NotifierJNI.cancelNotifierAlarm(m_notifier.get());
} finally {
m_processLock.unlock();
}
}
/**
* Sets the HAL notifier thread priority.
*
* <p>The HAL notifier thread is responsible for managing the FPGA's notifier interrupt and waking
* up user's Notifiers when it's their time to run. Giving the HAL notifier thread real-time
* priority helps ensure the user's real-time Notifiers, if any, are notified to run in a timely
* manner.
*
* @param realTime Set to true to set a real-time priority, false for standard priority.
* @param priority Priority to set the thread to. For real-time, this is 1-99 with 99 being
* highest. For non-real-time, this is forced to 0. See "man 7 sched" for more details.
* @return True on success.
*/
public static boolean setHALThreadPriority(boolean realTime, int priority) {
return NotifierJNI.setHALThreadPriority(realTime, priority);
}
}

107
wpilibj/src/main/java/org/wpilib/system/Resource.java Normal file
View File

@@ -0,0 +1,107 @@
// 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 edu.wpi.first.hal.util.AllocationException;
import edu.wpi.first.hal.util.CheckedAllocationException;
/**
* Track resources in the program. The Resource class is a convenient way of keeping track of
* allocated arbitrary resources in the program. Resources are just indices that have a lower and
* upper bound that are tracked by this class. In the library they are used for tracking allocation
* of hardware channels but this is purely arbitrary. The resource class does not do any actual
* allocation, but simply tracks if a given index is currently in use.
*
* <p><b>WARNING:</b> this should only be statically allocated. When the program loads into memory
* all the static constructors are called. At that time a linked list of all the "Resources" is
* created. Then, when the program actually starts - in the Robot constructor, all resources are
* initialized. This ensures that the program is restartable in memory without having to
* unload/reload.
*
* @deprecated Will be removed with no replacement.
*/
@Deprecated(forRemoval = true, since = "2025")
public final class Resource {
private static Resource resourceList;
private final boolean[] m_numAllocated;
private final int m_size;
private final Resource m_nextResource;
/** Clears all allocated resources. */
public static void restartProgram() {
for (Resource r = Resource.resourceList; r != null; r = r.m_nextResource) {
for (int i = 0; i < r.m_size; i++) {
r.m_numAllocated[i] = false;
}
}
}
/**
* Allocate storage for a new instance of Resource. Allocate a bool array of values that will get
* initialized to indicate that no resources have been allocated yet. The indices of the resources
* are 0..size-1.
*
* @param size The number of blocks to allocate
*/
public Resource(final int size) {
m_size = size;
m_numAllocated = new boolean[size];
for (int i = 0; i < size; i++) {
m_numAllocated[i] = false;
}
m_nextResource = Resource.resourceList;
Resource.resourceList = this;
}
/**
* Allocate a resource. When a resource is requested, mark it allocated. In this case, a free
* resource value within the range is located and returned after it is marked allocated.
*
* @return The index of the allocated block.
* @throws CheckedAllocationException If there are no resources available to be allocated.
*/
public int allocate() throws CheckedAllocationException {
for (int i = 0; i < m_size; i++) {
if (!m_numAllocated[i]) {
m_numAllocated[i] = true;
return i;
}
}
throw new CheckedAllocationException("No available resources");
}
/**
* Allocate a specific resource value. The user requests a specific resource value, i.e. channel
* number, and it is verified unallocated, then returned.
*
* @param index The resource to allocate
* @return The index of the allocated block
* @throws CheckedAllocationException If there are no resources available to be allocated.
*/
public int allocate(final int index) throws CheckedAllocationException {
if (index >= m_size || index < 0) {
throw new CheckedAllocationException("Index " + index + " out of range");
}
if (m_numAllocated[index]) {
throw new CheckedAllocationException("Resource at index " + index + " already allocated");
}
m_numAllocated[index] = true;
return index;
}
/**
* Free an allocated resource. After a resource is no longer needed, for example a destructor is
* called for a channel assignment class, this method will release the resource value, so it can
* be reused somewhere else in the program.
*
* @param index The index of the resource to free.
*/
public void free(final int index) {
if (!m_numAllocated[index]) {
throw new AllocationException("No resource available to be freed");
}
m_numAllocated[index] = false;
}
}

332
wpilibj/src/main/java/org/wpilib/system/RobotController.java Normal file
View File

@@ -0,0 +1,332 @@
// 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 static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Celsius;
import static edu.wpi.first.units.Units.Microseconds;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.HALUtil;
import edu.wpi.first.hal.PowerJNI;
import edu.wpi.first.hal.can.CANJNI;
import edu.wpi.first.hal.can.CANStatus;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Temperature;
import edu.wpi.first.units.measure.Time;
import edu.wpi.first.units.measure.Voltage;
import java.util.function.LongSupplier;
/** Contains functions for roboRIO functionality. */
public final class RobotController {
private static LongSupplier m_timeSource = RobotController::getFPGATime;
private RobotController() {
throw new UnsupportedOperationException("This is a utility class!");
}
/**
* Return the serial number of the roboRIO.
*
* @return The serial number of the roboRIO.
*/
public static String getSerialNumber() {
return HALUtil.getSerialNumber();
}
/**
* Return the comments from the roboRIO web interface.
*
* <p>The comments string is cached after the first call to this function on the RoboRIO - restart
* the robot code to reload the comments string after changing it in the web interface.
*
* @return the comments from the roboRIO web interface.
*/
public static String getComments() {
return HALUtil.getComments();
}
/**
* Returns the team number configured for the robot controller.
*
* @return team number, or 0 if not found.
*/
public static int getTeamNumber() {
return HALUtil.getTeamNumber();
}
/**
* Sets a new source to provide the clock time in microseconds. Changing this affects the return
* value of {@code getTime} in Java.
*
* @param supplier Function to return the time in microseconds.
*/
public static void setTimeSource(LongSupplier supplier) {
m_timeSource = supplier;
}
/**
* Read the microsecond timestamp. By default, the time is based on the FPGA hardware clock in
* microseconds since the FPGA started. However, the return value of this method may be modified
* to use any time base, including non-monotonic and non-continuous time bases.
*
* @return The current time in microseconds.
*/
public static long getTime() {
return m_timeSource.getAsLong();
}
/**
* Read the microsecond timestamp. By default, the time is based on the FPGA hardware clock in
* microseconds since the FPGA started. However, the return value of this method may be modified
* to use any time base, including non-monotonic and non-continuous time bases.
*
* @return The current time in a measure.
*/
public static Time getMeasureTime() {
return Microseconds.of(m_timeSource.getAsLong());
}
/**
* Read the microsecond timer from the FPGA.
*
* @return The current time in microseconds according to the FPGA.
*/
public static long getFPGATime() {
return HALUtil.getFPGATime();
}
/**
* Read the microsecond timer in a measure from the FPGA.
*
* @return The current time according to the FPGA in a measure.
*/
public static Time getMeasureFPGATime() {
return Microseconds.of(HALUtil.getFPGATime());
}
/**
* Read the battery voltage.
*
* @return The battery voltage in Volts.
*/
public static double getBatteryVoltage() {
return PowerJNI.getVinVoltage();
}
/**
* Read the battery voltage in a measure.
*
* @return The battery voltage in a measure.
*/
public static Voltage getMeasureBatteryVoltage() {
return Volts.of(PowerJNI.getVinVoltage());
}
/**
* Gets a value indicating whether the FPGA outputs are enabled. The outputs may be disabled if
* the robot is disabled or e-stopped, the watchdog has expired, or if the roboRIO browns out.
*
* @return True if the FPGA outputs are enabled.
*/
public static boolean isSysActive() {
return HAL.getSystemActive();
}
/**
* Check if the system is browned out.
*
* @return True if the system is browned out
*/
public static boolean isBrownedOut() {
return HAL.getBrownedOut();
}
/**
* Gets the number of times the system has been disabled due to communication errors with the
* Driver Station.
*
* @return number of disables due to communication errors.
*/
public static int getCommsDisableCount() {
return HAL.getCommsDisableCount();
}
/**
* Gets the current state of the Robot Signal Light (RSL).
*
* @return The current state of the RSL- true if on, false if off
*/
public static boolean getRSLState() {
return HAL.getRSLState();
}
/**
* Gets if the system time is valid.
*
* @return True if the system time is valid, false otherwise
*/
public static boolean isSystemTimeValid() {
return HAL.getSystemTimeValid();
}
/**
* Get the input voltage to the robot controller.
*
* @return The controller input voltage value in Volts
*/
public static double getInputVoltage() {
return PowerJNI.getVinVoltage();
}
/**
* Get the input voltage to the robot controller in a measure.
*
* @return The controller input voltage value in a measure.
*/
public static Voltage getMeasureInputVoltage() {
return Volts.of(PowerJNI.getVinVoltage());
}
/**
* Get the voltage of the 3.3V rail.
*
* @return The controller 3.3V rail voltage value in Volts
*/
public static double getVoltage3V3() {
return PowerJNI.getUserVoltage3V3();
}
/**
* Get the voltage in a measure of the 3.3V rail.
*
* @return The controller 3.3V rail voltage value in a measure.
*/
public static Voltage getMeasureVoltage3V3() {
return Volts.of(PowerJNI.getUserVoltage3V3());
}
/**
* Get the current output of the 3.3V rail.
*
* @return The controller 3.3V rail output current value in Amps
*/
public static double getCurrent3V3() {
return PowerJNI.getUserCurrent3V3();
}
/**
* Get the current output in a measure of the 3.3V rail.
*
* @return The controller 3.3V rail output current value in a measure.
*/
public static Current getMeasureCurrent3V3() {
return Amps.of(PowerJNI.getUserCurrent3V3());
}
/**
* Enables or disables the 3.3V rail.
*
* @param enabled whether to enable the 3.3V rail.
*/
public static void setEnabled3V3(boolean enabled) {
PowerJNI.setUserEnabled3V3(enabled);
}
/**
* Get the enabled state of the 3.3V rail. The rail may be disabled due to a controller brownout,
* a short circuit on the rail, or controller over-voltage.
*
* @return The controller 3.3V rail enabled value
*/
public static boolean getEnabled3V3() {
return PowerJNI.getUserActive3V3();
}
/**
* Get the count of the total current faults on the 3.3V rail since the code started.
*
* @return The number of faults
*/
public static int getFaultCount3V3() {
return PowerJNI.getUserCurrentFaults3V3();
}
/** Reset the overcurrent fault counters for all user rails to 0. */
public static void resetRailFaultCounts() {
PowerJNI.resetUserCurrentFaults();
}
/**
* Get the current brownout voltage setting.
*
* @return The brownout voltage
*/
public static double getBrownoutVoltage() {
return PowerJNI.getBrownoutVoltage();
}
/**
* Get the current brownout voltage setting in a measure.
*
* @return The brownout voltage in a measure.
*/
public static Voltage getMeasureBrownoutVoltage() {
return Volts.of(PowerJNI.getBrownoutVoltage());
}
/**
* Set the voltage the roboRIO will brownout and disable all outputs.
*
* <p>Note that this only does anything on the roboRIO 2. On the roboRIO it is a no-op.
*
* @param brownoutVoltage The brownout voltage
*/
public static void setBrownoutVoltage(double brownoutVoltage) {
PowerJNI.setBrownoutVoltage(brownoutVoltage);
}
/**
* Set the voltage in a measure the roboRIO will brownout and disable all outputs.
*
* <p>Note that this only does anything on the roboRIO 2. On the roboRIO it is a no-op.
*
* @param brownoutVoltage The brownout voltage in a measure
*/
public static void setBrownoutVoltage(Voltage brownoutVoltage) {
PowerJNI.setBrownoutVoltage(brownoutVoltage.baseUnitMagnitude());
}
/**
* Get the current CPU temperature in degrees Celsius.
*
* @return current CPU temperature in degrees Celsius
*/
public static double getCPUTemp() {
return PowerJNI.getCPUTemp();
}
/**
* Get the current CPU temperature in a measure.
*
* @return current CPU temperature in a measure.
*/
public static Temperature getMeasureCPUTemp() {
return Celsius.of(PowerJNI.getCPUTemp());
}
/**
* Get the current status of the CAN bus.
*
* @param busId The bus ID
* @return The status of the CAN bus
*/
public static CANStatus getCANStatus(int busId) {
CANStatus status = new CANStatus();
CANJNI.getCANStatus(busId, status);
return status;
}
}

41
wpilibj/src/main/java/org/wpilib/system/RuntimeType.java Normal file
View File

@@ -0,0 +1,41 @@
// 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 edu.wpi.first.hal.HALUtil;
/** Runtime type. */
public enum RuntimeType {
/** roboRIO 1.0. */
kRoboRIO(HALUtil.RUNTIME_ROBORIO),
/** roboRIO 2.0. */
kRoboRIO2(HALUtil.RUNTIME_ROBORIO2),
/** Simulation runtime. */
kSimulation(HALUtil.RUNTIME_SIMULATION),
/** SystemCore. */
kSystemCore(HALUtil.RUNTIME_SYSTEMCORE);
/** RuntimeType value. */
public final int value;
RuntimeType(int value) {
this.value = value;
}
/**
* Construct a runtime type from an int value.
*
* @param type Runtime type as int
* @return Matching runtime type
*/
public static RuntimeType getValue(int type) {
return switch (type) {
case HALUtil.RUNTIME_ROBORIO -> RuntimeType.kRoboRIO;
case HALUtil.RUNTIME_ROBORIO2 -> RuntimeType.kRoboRIO2;
case HALUtil.RUNTIME_SYSTEMCORE -> RuntimeType.kSystemCore;
default -> RuntimeType.kSimulation;
};
}
}

119
wpilibj/src/main/java/org/wpilib/system/SensorUtil.java Normal file
View File

@@ -0,0 +1,119 @@
// 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 edu.wpi.first.hal.AnalogJNI;
import edu.wpi.first.hal.ConstantsJNI;
import edu.wpi.first.hal.DIOJNI;
import edu.wpi.first.hal.PWMJNI;
import edu.wpi.first.hal.PortsJNI;
/**
* Stores most recent status information as well as containing utility functions for checking
* channels and error processing.
*/
public final class SensorUtil {
/** Ticks per microsecond. */
public static final int kSystemClockTicksPerMicrosecond =
ConstantsJNI.getSystemClockTicksPerMicrosecond();
/** Number of digital channels per Systemcore. */
public static final int kDigitalChannels = PortsJNI.getNumDigitalChannels();
/** Number of analog input channels per Systemcore. */
public static final int kAnalogInputChannels = PortsJNI.getNumAnalogInputs();
/** Number of solenoid channels per module. */
public static final int kCTRESolenoidChannels = PortsJNI.getNumCTRESolenoidChannels();
/** Number of PWM channels per Systemcore. */
public static final int kPwmChannels = PortsJNI.getNumPWMChannels();
/** Number of power distribution channels per PDP. */
public static final int kCTREPDPChannels = PortsJNI.getNumCTREPDPChannels();
/** Number of power distribution modules per PDP. */
public static final int kCTREPDPModules = PortsJNI.getNumCTREPDPModules();
/** Number of PCM Modules. */
public static final int kCTREPCMModules = PortsJNI.getNumCTREPCMModules();
/** Number of power distribution channels per PH. */
public static final int kREVPHChannels = PortsJNI.getNumREVPHChannels();
/** Number of PH modules. */
public static final int kREVPHModules = PortsJNI.getNumREVPHModules();
/**
* Check that the digital channel number is valid. Verify that the channel number is one of the
* legal channel numbers. Channel numbers are 0-based.
*
* @param channel The channel number to check.
*/
public static void checkDigitalChannel(final int channel) {
if (!DIOJNI.checkDIOChannel(channel)) {
String buf =
"Requested DIO channel is out of range. Minimum: 0, Maximum: "
+ kDigitalChannels
+ ", Requested: "
+ channel;
throw new IllegalArgumentException(buf);
}
}
/**
* Check that the PWM channel number is valid. Verify that the channel number is one of the legal
* channel numbers. Channel numbers are 0-based.
*
* @param channel The channel number to check.
*/
public static void checkPWMChannel(final int channel) {
if (!PWMJNI.checkPWMChannel(channel)) {
String buf =
"Requested PWM channel is out of range. Minimum: 0, Maximum: "
+ kPwmChannels
+ ", Requested: "
+ channel;
throw new IllegalArgumentException(buf);
}
}
/**
* Check that the analog input number is value. Verify that the analog input number is one of the
* legal channel numbers. Channel numbers are 0-based.
*
* @param channel The channel number to check.
*/
public static void checkAnalogInputChannel(final int channel) {
if (!AnalogJNI.checkAnalogInputChannel(channel)) {
String buf =
"Requested analog input channel is out of range. Minimum: 0, Maximum: "
+ kAnalogInputChannels
+ ", Requested: "
+ channel;
throw new IllegalArgumentException(buf);
}
}
/**
* Get the number of the default solenoid module.
*
* @return The number of the default solenoid module.
*/
public static int getDefaultCTREPCMModule() {
return 0;
}
/**
* Get the number of the default solenoid module.
*
* @return The number of the default solenoid module.
*/
public static int getDefaultREVPHModule() {
return 1;
}
private SensorUtil() {}
}

23
wpilibj/src/main/java/org/wpilib/system/SystemServer.java Normal file
View File

@@ -0,0 +1,23 @@
// 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 edu.wpi.first.hal.SystemServerJNI;
import edu.wpi.first.networktables.NetworkTableInstance;
/** Class to get system server NT instance. */
public final class SystemServer {
/**
* Gets the system server NT Instance.
*
* @return NT Instance
*/
public static NetworkTableInstance getSystemServer() {
return NetworkTableInstance.fromNativeHandle(SystemServerJNI.getSystemServerHandle());
}
/** Utility Class. */
private SystemServer() {}
}

43
wpilibj/src/main/java/org/wpilib/system/Threads.java Normal file
View File

@@ -0,0 +1,43 @@
// 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 edu.wpi.first.hal.ThreadsJNI;
/** Thread utility functions. */
public final class Threads {
/**
* Get the thread priority for the current thread.
*
* @return The current thread priority. For real-time, this is 1-99 with 99 being highest. For
* non-real-time, this is 0. See "man 7 sched" for details.
*/
public static int getCurrentThreadPriority() {
return ThreadsJNI.getCurrentThreadPriority();
}
/**
* Get if the current thread is real-time.
*
* @return If the current thread is real-time.
*/
public static boolean getCurrentThreadIsRealTime() {
return ThreadsJNI.getCurrentThreadIsRealTime();
}
/**
* Sets the thread priority for the current thread.
*
* @param realTime Set to true to set a real-time priority, false for standard priority.
* @param priority Priority to set the thread to. For real-time, this is 1-99 with 99 being
* highest. For non-real-time, this is forced to 0. See "man 7 sched" for details.
* @return True on success.
*/
public static boolean setCurrentThreadPriority(boolean realTime, int priority) {
return ThreadsJNI.setCurrentThreadPriority(realTime, priority);
}
private Threads() {}
}

198
wpilibj/src/main/java/org/wpilib/system/Timer.java Normal file
View File

@@ -0,0 +1,198 @@
// 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 static edu.wpi.first.units.Units.Seconds;
import edu.wpi.first.units.measure.Time;
/**
* A timer class.
*
* <p>Note that if the user calls SimHooks.restartTiming(), they should also reset the timer so
* get() won't return a negative duration.
*/
public class Timer {
/**
* Return the clock time in seconds. By default, the time is based on the FPGA hardware clock in
* seconds since the FPGA started. However, the return value of this method may be modified to use
* any time base, including non-monotonic time bases.
*
* @return Robot running time in seconds.
*/
public static double getTimestamp() {
return RobotController.getTime() / 1000000.0;
}
/**
* Return the system clock time in seconds. Return the time from the FPGA hardware clock in
* seconds since the FPGA started.
*
* @return Robot running time in seconds.
*/
public static double getFPGATimestamp() {
return RobotController.getFPGATime() / 1000000.0;
}
/**
* Return the approximate match time. The FMS does not send an official match time to the robots,
* but does send an approximate match time. The value will count down the time remaining in the
* current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
* dispute ref calls or guarantee that a function will trigger before the match ends) The Practice
* Match function of the DS approximates the behavior seen on the field.
*
* @return Time remaining in current match period (auto or teleop) in seconds
*/
public static double getMatchTime() {
return DriverStation.getMatchTime();
}
/**
* Pause the execution of the thread for a specified period of time. Motors will continue to run
* at their last assigned values, and sensors will continue to update. Only the task containing
* the wait will pause until the wait time is expired.
*
* @param period Length of time to pause
*/
public static void delay(final Time period) {
delay(period.in(Seconds));
}
/**
* Pause the execution of the thread for a specified period of time given in seconds. Motors will
* continue to run at their last assigned values, and sensors will continue to update. Only the
* task containing the wait will pause until the wait time is expired.
*
* @param seconds Length of time to pause
*/
public static void delay(final double seconds) {
try {
Thread.sleep((long) (seconds * 1e3));
} catch (final InterruptedException ex) {
Thread.currentThread().interrupt();
}
}
private double m_startTime;
private double m_accumulatedTime;
private boolean m_running;
/** Timer constructor. */
public Timer() {
reset();
}
private double getMsClock() {
return RobotController.getTime() / 1000.0;
}
/**
* Get the current time from the timer. If the clock is running it is derived from the current
* system clock the start time stored in the timer class. If the clock is not running, then return
* the time when it was last stopped.
*
* @return Current time value for this timer in seconds
*/
public double get() {
if (m_running) {
return m_accumulatedTime + (getMsClock() - m_startTime) / 1000.0;
} else {
return m_accumulatedTime;
}
}
/**
* Reset the timer by setting the time to 0.
*
* <p>Make the timer startTime the current time so new requests will be relative now.
*/
public final void reset() {
m_accumulatedTime = 0;
m_startTime = getMsClock();
}
/**
* Start the timer running. Just set the running flag to true indicating that all time requests
* should be relative to the system clock. Note that this method is a no-op if the timer is
* already running.
*/
public void start() {
if (!m_running) {
m_startTime = getMsClock();
m_running = true;
}
}
/**
* Restart the timer by stopping the timer, if it is not already stopped, resetting the
* accumulated time, then starting the timer again. If you want an event to periodically reoccur
* at some time interval from the start time, consider using advanceIfElapsed() instead.
*/
public void restart() {
if (m_running) {
stop();
}
reset();
start();
}
/**
* Stop the timer. This computes the time as of now and clears the running flag, causing all
* subsequent time requests to be read from the accumulated time rather than looking at the system
* clock.
*/
public void stop() {
m_accumulatedTime = get();
m_running = false;
}
/**
* Check if the period specified has passed.
*
* @param period The period to check.
* @return Whether the period has passed.
*/
public boolean hasElapsed(Time period) {
return hasElapsed(period.in(Seconds));
}
/**
* Check if the period specified has passed.
*
* @param seconds The period to check in seconds.
* @return Whether the period has passed.
*/
public boolean hasElapsed(double seconds) {
return get() >= seconds;
}
/**
* Check if the period specified has passed and if it has, advance the start time by that period.
* This is useful to decide if it's time to do periodic work without drifting later by the time it
* took to get around to checking.
*
* @param seconds The period to check.
* @return Whether the period has passed.
*/
public boolean advanceIfElapsed(double seconds) {
if (get() >= seconds) {
// Advance the start time by the period.
// Don't set it to the current time... we want to avoid drift.
m_startTime += seconds * 1000;
return true;
} else {
return false;
}
}
/**
* Whether the timer is currently running.
*
* @return true if running.
*/
public boolean isRunning() {
return m_running;
}
}

84
wpilibj/src/main/java/org/wpilib/system/Tracer.java Normal file
View File

@@ -0,0 +1,84 @@
// 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;
import java.util.Map;
import java.util.function.Consumer;
/**
* 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()}.
*
* <p>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 long kMinPrintPeriod = 1000000; // microseconds
private long m_lastEpochsPrintTime; // microseconds
private long m_startTime; // microseconds
private final Map<String, Long> 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().
*
* <p>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.
*
* <p>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.
*
* <p>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<String> 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());
}
}
}
}

280
wpilibj/src/main/java/org/wpilib/system/Watchdog.java Normal file
View File

@@ -0,0 +1,280 @@
// 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 static edu.wpi.first.units.Units.Seconds;
import edu.wpi.first.hal.NotifierJNI;
import edu.wpi.first.units.measure.Time;
import java.io.Closeable;
import java.util.PriorityQueue;
import java.util.concurrent.locks.ReentrantLock;
/**
* A class that's a wrapper around a watchdog timer.
*
* <p>When the timer expires, a message is printed to the console and an optional user-provided
* callback is invoked.
*
* <p>The watchdog is initialized disabled, so the user needs to call enable() before use.
*/
public class Watchdog implements Closeable, Comparable<Watchdog> {
// Used for timeout print rate-limiting
private static final long kMinPrintPeriod = (long) 1e6; // μs
private double m_startTime;
private double m_timeout;
private double m_expirationTime;
private final Runnable m_callback;
private double m_lastTimeoutPrint; // s
boolean m_isExpired;
boolean m_suppressTimeoutMessage;
private final Tracer m_tracer;
private static final PriorityQueue<Watchdog> m_watchdogs = new PriorityQueue<>();
private static ReentrantLock m_queueMutex = new ReentrantLock();
private static int m_notifier;
static {
m_notifier = NotifierJNI.initializeNotifier();
NotifierJNI.setNotifierName(m_notifier, "Watchdog");
startDaemonThread(Watchdog::schedulerFunc);
}
/**
* Watchdog constructor.
*
* @param timeout The watchdog's timeout in seconds with microsecond resolution.
* @param callback This function is called when the timeout expires.
*/
public Watchdog(double timeout, Runnable callback) {
m_timeout = timeout;
m_callback = callback;
m_tracer = new Tracer();
}
/**
* Watchdog constructor.
*
* @param timeout The watchdog's timeout with microsecond resolution.
* @param callback This function is called when the timeout expires.
*/
public Watchdog(Time timeout, Runnable callback) {
this(timeout.in(Seconds), callback);
}
@Override
public void close() {
disable();
}
@Override
public boolean equals(Object obj) {
return obj instanceof Watchdog watchdog
&& Double.compare(m_expirationTime, watchdog.m_expirationTime) == 0;
}
@Override
public int hashCode() {
return Double.hashCode(m_expirationTime);
}
@Override
public int compareTo(Watchdog rhs) {
// Elements with sooner expiration times are sorted as lesser. The head of
// Java's PriorityQueue is the least element.
return Double.compare(m_expirationTime, rhs.m_expirationTime);
}
/**
* Returns the time in seconds since the watchdog was last fed.
*
* @return The time in seconds since the watchdog was last fed.
*/
public double getTime() {
return Timer.getFPGATimestamp() - m_startTime;
}
/**
* Sets the watchdog's timeout.
*
* @param timeout The watchdog's timeout in seconds with microsecond resolution.
*/
public void setTimeout(double timeout) {
m_startTime = Timer.getFPGATimestamp();
m_tracer.clearEpochs();
m_queueMutex.lock();
try {
m_timeout = timeout;
m_isExpired = false;
m_watchdogs.remove(this);
m_expirationTime = m_startTime + m_timeout;
m_watchdogs.add(this);
updateAlarm();
} finally {
m_queueMutex.unlock();
}
}
/**
* Returns the watchdog's timeout in seconds.
*
* @return The watchdog's timeout in seconds.
*/
public double getTimeout() {
m_queueMutex.lock();
try {
return m_timeout;
} finally {
m_queueMutex.unlock();
}
}
/**
* Returns true if the watchdog timer has expired.
*
* @return True if the watchdog timer has expired.
*/
public boolean isExpired() {
m_queueMutex.lock();
try {
return m_isExpired;
} finally {
m_queueMutex.unlock();
}
}
/**
* 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.
*
* <p>This also enables the timer if it was previously disabled.
*/
public void reset() {
enable();
}
/** Enables the watchdog timer. */
public void enable() {
m_startTime = Timer.getFPGATimestamp();
m_tracer.clearEpochs();
m_queueMutex.lock();
try {
m_isExpired = false;
m_watchdogs.remove(this);
m_expirationTime = m_startTime + m_timeout;
m_watchdogs.add(this);
updateAlarm();
} finally {
m_queueMutex.unlock();
}
}
/** Disables the watchdog timer. */
public void disable() {
m_queueMutex.lock();
try {
m_watchdogs.remove(this);
updateAlarm();
} finally {
m_queueMutex.unlock();
}
}
/**
* Enable or disable suppression of the generic timeout message.
*
* <p>This may be desirable if the user-provided callback already prints a more specific message.
*
* @param suppress Whether to suppress generic timeout message.
*/
public void suppressTimeoutMessage(boolean suppress) {
m_suppressTimeoutMessage = suppress;
}
@SuppressWarnings("resource")
private static void updateAlarm() {
if (m_watchdogs.isEmpty()) {
NotifierJNI.cancelNotifierAlarm(m_notifier);
} else {
NotifierJNI.updateNotifierAlarm(
m_notifier, (long) (m_watchdogs.peek().m_expirationTime * 1e6));
}
}
private static Thread startDaemonThread(Runnable target) {
Thread inst = new Thread(target);
inst.setDaemon(true);
inst.start();
return inst;
}
private static void schedulerFunc() {
while (!Thread.currentThread().isInterrupted()) {
long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
if (curTime == 0) {
break;
}
m_queueMutex.lock();
try {
if (m_watchdogs.isEmpty()) {
continue;
}
// If the condition variable timed out, that means a Watchdog timeout
// has occurred, so call its timeout function.
Watchdog watchdog = m_watchdogs.poll();
double now = curTime * 1e-6;
if (now - watchdog.m_lastTimeoutPrint > kMinPrintPeriod) {
watchdog.m_lastTimeoutPrint = now;
if (!watchdog.m_suppressTimeoutMessage) {
DriverStation.reportWarning(
String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout), false);
}
}
// Set expiration flag before calling the callback so any
// manipulation of the flag in the callback (e.g., calling
// Disable()) isn't clobbered.
watchdog.m_isExpired = true;
m_queueMutex.unlock();
watchdog.m_callback.run();
m_queueMutex.lock();
updateAlarm();
} finally {
m_queueMutex.unlock();
}
}
}
}