WIP - try to port monitors but the packages can't see each other yet …

…so it's all broken

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'

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;
+    }
+}

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());
+    // }
+}

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')

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<Monitor> registeredMonitors = new ArrayList<Monitor>();
+
+    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.
+     *
+     * <p>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();
+                });
+    }
+}