"finished" threshold and plateau tests

src/main/java/common/utility/tester/Plateau.java

@@ -0,0 +1,93 @@
+package common.utility.tester;
+
+import static edu.wpi.first.wpilibj2.command.Commands.*;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+import common.utility.tester.Tester.UnitTest;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class Plateau extends UnitTest {
+    private String testName;
+    private double plateau;
+    private double tolerance;
+    private Timer timer;
+    private boolean hasDelayed;
+    private static double timeout = 30;
+
+    /**
+     * 
+     * @param testName Name of test.
+     * @param plateau How long the pass condition must be true for.
+     * @param measurement Source of measurement
+     * @param expected Expected value
+     * @param tolerance Maximum error.
+     */
+    public Plateau(String testName, double plateau, DoubleSupplier measurement, double expected, double tolerance) {
+        this(testName, plateau, () -> Math.abs(measurement.getAsDouble() - expected), tolerance, () -> (Math.abs(measurement.getAsDouble() - expected) < tolerance));
+    }
+
+    /**
+     * 
+     * @param testName Name of test.
+     * @param plateau How long the pass condition must be true for.
+     * @param error Difference from expected value.
+     * @param tolerance Maximum error.
+     */
+    public Plateau(String testName, double plateau, DoubleSupplier error, double tolerance) {
+        this(testName, plateau, error, tolerance, () -> Math.abs(error.getAsDouble()) < tolerance);
+    }
+
+    /**
+     * 
+     * @param testName Name of test.
+     * @param plateau How long the pass condition must be true for.
+     * @param error Difference from expected value.
+     * @param tolerance Maximum error.
+     */
+    public Plateau(String testName, double plateau, DoubleSupplier error, double tolerance, BooleanSupplier passCondition) {
+        super(
+            testName,
+            race(run(()-> error.getAsDouble()), waitSeconds(timeout)),
+            passCondition
+        );
+        this.plateau = plateau;
+        timer = new Timer();
+        hasDelayed = false;
+    }
+
+
+    @Override
+    public void initialize() {
+        super.initialize();
+        timer.restart();
+        hasDelayed = false;
+    }
+
+    @Override
+    public void execute() {
+        super.execute();
+        if (!hasDelayed) {
+            if (timer.hasElapsed(1)) {
+                hasDelayed = true;
+                timer.reset();
+            }
+            return;
+        }
+        if (!passCondition.getAsBoolean()) timer.reset();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        super.end(interrupted);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return super.isFinished() || (timer.hasElapsed(plateau) && hasDelayed);
+    }
+
+
+}

src/main/java/common/utility/tester/Tester.java

@@ -22,7 +22,6 @@ public static class UnitTest extends Command {
         protected Command command;
         protected BooleanSupplier passCondition;
         protected TestState testState;
-
         /**
          * Creates a new Unit Test.
          * @param testName Name of the test.

src/main/java/common/utility/tester/Threshold.java

@@ -0,0 +1,84 @@
+package common.utility.tester;
+
+import static edu.wpi.first.wpilibj2.command.Commands.*;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+import common.hardware.motorcontroller.NAR_Motor;
+import common.utility.Log;
+import common.utility.tester.Tester.TestState;
+import common.utility.tester.Tester.UnitTest;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+public class Threshold extends UnitTest {
+    public String testName;
+    private double plateau; //not sure if this is necessary. do we just want it to reach the threshold or does it have to HOLD it?
+    private boolean hasDelayed; //sometimes, i assume, the system will delay for one second; so we have to take that into account just in case it happens?
+    private Timer timer;
+    private static double timeout = 30;
+
+    /**
+     * Creates a Threshold test.
+     * @param testName Name of the test.
+     * @param measurement Source of measurement.
+     * @param threshold Threshold that should be reached.
+     */
+    public Threshold(String testName, DoubleSupplier measurement, double threshold) {
+        this(testName, measurement, ()-> measurement.getAsDouble() > threshold);
+    }
+
+
+    /**
+     * Creates a current test.
+     * @param testName Name of the test.
+     * @param measurement Source of measurement.
+     * @param threshold Threshold that should be reached.
+     * @param passCondition Condition for the test to pass.
+     */
+    public Threshold(
+            String testName, 
+            DoubleSupplier measurement,
+            BooleanSupplier passCondition) {
+        super(
+            testName, 
+            race(run(() -> measurement.getAsDouble()), waitSeconds(timeout)),
+            passCondition
+        );
+        timer = new Timer();
+        hasDelayed = false;
+    }
+
+    @Override
+    public void initialize() {
+        super.initialize();
+        timer.restart();
+        hasDelayed = false;
+    }
+
+    @Override
+    public void execute() {
+        super.execute();
+        if (!hasDelayed) {
+            if (timer.hasElapsed(1)) {
+                hasDelayed = true;
+                timer.reset();
+            }
+            return;
+        }
+        if (!passCondition.getAsBoolean()) timer.reset();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        super.end(interrupted);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return super.isFinished() || (hasDelayed);
+    }
+
+}

src/main/java/common/utility/tester/Time.java

@@ -0,0 +1,5 @@
+package common.utility.tester;
+
+public class Time {
+
+}