diff --git a/monitors/build.gradle b/monitors/build.gradle new file mode 100644 index 0000000..646994e --- /dev/null +++ b/monitors/build.gradle @@ -0,0 +1,72 @@ +/* + * This file was generated by the Gradle 'init' task. + * + * This generated file contains a sample Java library project to get you started. + * For more details on building Java & JVM projects, please refer to https://docs.gradle.org/8.7/userguide/building_java_projects.html in the Gradle documentation. + */ + +plugins { + // Apply the java-library plugin for API and implementation separation. + id 'java-library' + id "com.diffplug.spotless" version "6.24.0" + +} + +repositories { + // Use Maven Central for resolving dependencies. + mavenCentral() +} + +spotless { +// optional: limit format enforcement to just the files changed by this feature branch +ratchetFrom 'origin/main' + +format 'misc', { + // define the files to apply `misc` to + target '*.gradle', '.gitattributes', '.gitignore' + + // define the steps to apply to those files + trimTrailingWhitespace() + indentWithTabs() // or spaces. Takes an integer argument if you don't like 4 + endWithNewline() +} +java { + // don't need to set target, it is inferred from java + // Allow ignoring certain parts in formatting. + toggleOffOn() + // apply a specific flavor of google-java-format + googleJavaFormat('1.19.2').aosp().reflowLongStrings() + // fix formatting of type annotations + formatAnnotations() +} +} + +compileJava.dependsOn 'spotlessApply' + + +dependencies { + // Use JUnit Jupiter for testing. + testImplementation libs.junit.jupiter + + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + // This dependency is exported to consumers, that is to say found on their compile classpath. + api libs.commons.math3 + + // This dependency is used internally, and not exposed to consumers on their own compile classpath. + implementation libs.guava +} + +// Apply a specific Java toolchain to ease working on different environments. +java { + toolchain { + languageVersion = JavaLanguageVersion.of(17) + } +} + +tasks.named('test') { + // Use JUnit Platform for unit tests. + useJUnitPlatform() +} + +compileJava.dependsOn 'spotlessApply' diff --git a/monitors/src/main/java/coppercore/monitors/Monitor.java b/monitors/src/main/java/coppercore/monitors/Monitor.java new file mode 100644 index 0000000..0744573 --- /dev/null +++ b/monitors/src/main/java/coppercore/monitors/Monitor.java @@ -0,0 +1,73 @@ +package coppercore.monitors; + +import java.util.function.BooleanSupplier; + +public class Monitor { + String name; // Name to log the status of the monitor under + boolean sticky; // Should the monitor still report a fault after conditions return to normal? + double timeToFault; // How long the value can be unacceptable before a fault occurs + BooleanSupplier isStateValid; // Supplier with which to check whether the value is acceptable + Runnable faultCallback; // Function to call when the fault happens + + double triggeredTime = 0.0; // Timestamp when monitor was first triggered + // If this value is zero, the monitor has been triggered for less than 1 tick + + boolean triggered = false; // Is the value currently unnacceptable? + boolean faulted = false; // Has the monitor detected a fault? + + public Monitor( + String name, + boolean sticky, + BooleanSupplier isStateValid, + double timeToFault, + Runnable faultCallback) { + this.name = name; + this.sticky = sticky; + this.timeToFault = timeToFault; + this.isStateValid = isStateValid; + + this.faultCallback = faultCallback; + } + + public void periodic(double currentTimeSeconds) { + // currentTimeSeconds doesn't need to be from a specific point in history + // As long as the reference point is always the same, it could be from + // the robot being turned on, initialized, etc. + + triggered = !isStateValid.getAsBoolean(); + if (triggered) { + if (triggeredTime == 0.0) { + triggeredTime = currentTimeSeconds; + } + + if (currentTimeSeconds - triggeredTime > timeToFault) { + faulted = true; + } + } else { + if (!sticky) { + faulted = false; + } + + triggeredTime = 0.0; + } + if (faulted) { + faultCallback.run(); + } + + // TODO: Move logging to MonitoredSubsystem under wpi interface + // Logger.recordOutput("monitors/" + name + "/triggered", triggered); + // Logger.recordOutput("monitors/" + name + "/faulted", faulted); + } + + public boolean isFaulted() { + return faulted; + } + + public boolean isTriggered() { + return triggered; + } + + public void resetStickyFault() { + faulted = false; + } +} diff --git a/monitors/src/test/java/MonitorTests.java b/monitors/src/test/java/MonitorTests.java new file mode 100644 index 0000000..4543f17 --- /dev/null +++ b/monitors/src/test/java/MonitorTests.java @@ -0,0 +1,73 @@ +package coppercore.monitors.test; + +public class MonitorTests { + // TODO: FIGURE OUT HOW TO TEST THIS + // Callbacks are hard :( + + // Mimics a changing robot state + // boolean isStateValid = true; + + // @Test + // public void nonStickyTest() { + // Monitor exampleMonitor = + // new Monitor("exampleMonitor", false, () -> isStateValid, 1.0, () -> {}); + + // exampleMonitor.periodic(0.0); + // Assertions.assertFalse(exampleMonitor.isFaulted()); + // Assertions.assertFalse(exampleMonitor.isTriggered()); + + // isStateValid = false; + // exampleMonitor.periodic(1.0); + // Assertions.assertFalse(exampleMonitor.isFaulted()); + // Assertions.assertFalse(exampleMonitor.isTriggered()); + + // exampleMonitor.periodic(1.25); + // Assertions.assertFalse(exampleMonitor.isFaulted()); + // Assertions.assertFalse(exampleMonitor.isTriggered()); + + // exampleMonitor.periodic(2.0); + // Assertions.assertTrue(exampleMonitor.isFaulted()); + // Assertions.assertTrue(exampleMonitor.isTriggered()); + + // isStateValid = true; + // exampleMonitor.periodic(3.0); + // Assertions.assertFalse(exampleMonitor.isFaulted()); + // Assertions.assertFalse(exampleMonitor.isTriggered()); + // } + + // @Test + // public void stickyTest() { + // // Mimics a changing robot state + // boolean isStateValid = true; + + // Monitor exampleMonitor = + // new Monitor( + // "exampleMonitor", + // true, + // () -> isStateValid, + // 1.0, + // () -> {}); // TODO: Figure out how to test callback + + // exampleMonitor.periodic(0.0); + // Assertions.assertFalse(exampleMonitor.isFaulted()); + // Assertions.assertFalse(exampleMonitor.isTriggered()); + + // isStateValid = false; + // exampleMonitor.periodic(1.0); + // Assertions.assertFalse(exampleMonitor.isFaulted()); + // Assertions.assertFalse(exampleMonitor.isTriggered()); + + // exampleMonitor.periodic(1.25); + // Assertions.assertFalse(exampleMonitor.isFaulted()); + // Assertions.assertFalse(exampleMonitor.isTriggered()); + + // exampleMonitor.periodic(2.0); + // Assertions.assertTrue(exampleMonitor.isFaulted()); + // Assertions.assertTrue(exampleMonitor.isTriggered()); + + // isStateValid = true; + // exampleMonitor.periodic(3.0); + // Assertions.assertTrue(exampleMonitor.isFaulted()); + // Assertions.assertFalse(exampleMonitor.isTriggered()); + // } +} diff --git a/settings.gradle b/settings.gradle index 2d5f417..3adee0e 100644 --- a/settings.gradle +++ b/settings.gradle @@ -11,4 +11,4 @@ plugins { } rootProject.name = 'coppercore' -include('geometry', 'wpi_interface', 'math', 'controls', "parameter_tools") +include('geometry', 'wpi_interface', 'math', 'controls', "parameter_tools", 'monitors') diff --git a/wpi_interface/src/main/java/coppercore/wpi_interface/MonitoredSubsystem.java b/wpi_interface/src/main/java/coppercore/wpi_interface/MonitoredSubsystem.java new file mode 100644 index 0000000..da9e239 --- /dev/null +++ b/wpi_interface/src/main/java/coppercore/wpi_interface/MonitoredSubsystem.java @@ -0,0 +1,40 @@ +package coppercore.wpi_interface; + +import coppercore.monitors.Monitor; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.ArrayList; +import java.util.List; + +public class MonitoredSubsystem extends SubsystemBase { + private List registeredMonitors = new ArrayList(); + + public void addMonitor(Monitor monitor) { + registeredMonitors.add(monitor); + } + + @Override + public void periodic() { + monitoredPeriodic(); + runMonitors(); + } + + /** + * OVERRIDE ME! This function is called every time the subsystem's periodic function is called. + * However, MonitoredSubsytem automatically checks monitors during every periodic run. + * Therefore, this method should be overridden as a replacement for the normal periodic function + * in the implementation of the subsystem. + * + *

This method is called periodically by the {@link CommandScheduler}. Useful for updating + * subsystem-specific state that you don't want to offload to a {@link Command}. Teams should + * try to be consistent within their own codebases about which responsibilities will be handled + * by Commands, and which will be handled here. + */ + public void monitoredPeriodic() {} + + private void runMonitors() { + registeredMonitors.forEach( + monitor -> { + monitor.periodic(); + }); + } +}