fault logger and input stream updates

src/main/java/org/sciborgs1155/lib/FaultLogger.java

@@ -42,11 +42,6 @@ public String toString() {
     }
   }
 
-  @FunctionalInterface
-  public static interface FaultReporter {
-    void report();
-  }
-
   /**
    * The type of fault, used for detecting whether the fallible is in a failure state and displaying
    * to NetworkTables.
@@ -79,8 +74,7 @@ public void set(Set<Fault> faults) {
   }
 
   // DATA
-  private static final List<FaultReporter> faultReporters = new ArrayList<>();
-  private static final Set<Fault> newFaults = new HashSet<>();
+  private static final List<Supplier<Optional<Fault>>> faultReporters = new ArrayList<>();
   private static final Set<Fault> activeFaults = new HashSet<>();
   private static final Set<Fault> totalFaults = new HashSet<>();
 
@@ -91,23 +85,20 @@ public void set(Set<Fault> faults) {
 
   /** Polls registered fallibles. This method should be called periodically. */
   public static void update() {
-    activeFaults.clear();
-
-    faultReporters.forEach(FaultReporter::report);
-    activeFaults.addAll(newFaults);
-    newFaults.clear();
+    faultReporters.forEach(r -> r.get().ifPresent(fault -> report(fault)));
 
     totalFaults.addAll(activeFaults);
 
     activeAlerts.set(activeFaults);
     totalAlerts.set(totalFaults);
+
+    activeFaults.clear();
   }
 
   /** Clears total faults. */
   public static void clear() {
     totalFaults.clear();
     activeFaults.clear();
-    newFaults.clear();
   }
 
   /** Clears fault suppliers. */
@@ -139,7 +130,7 @@ public static Set<Fault> totalFaults() {
    * @param fault The fault to report.
    */
   public static void report(Fault fault) {
-    newFaults.add(fault);
+    activeFaults.add(fault);
     switch (fault.type) {
       case ERROR -> DriverStation.reportError(fault.toString(), false);
       case WARNING -> DriverStation.reportWarning(fault.toString(), false);
@@ -164,7 +155,7 @@ public static void report(String name, String description, FaultType type) {
    * @param supplier A supplier of an optional fault.
    */
   public static void register(Supplier<Optional<Fault>> supplier) {
-    faultReporters.add(() -> supplier.get().ifPresent(FaultLogger::report));
+    faultReporters.add(supplier);
   }
 
   /**
@@ -176,12 +167,11 @@ public static void register(Supplier<Optional<Fault>> supplier) {
    */
   public static void register(
       BooleanSupplier condition, String name, String description, FaultType type) {
-    faultReporters.add(
-        () -> {
-          if (condition.getAsBoolean()) {
-            report(name, description, type);
-          }
-        });
+    register(
+        () ->
+            condition.getAsBoolean()
+                ? Optional.of(new Fault(name, description, type))
+                : Optional.empty());
   }
 
   /**
@@ -190,14 +180,9 @@ public static void register(
    * @param spark The Spark Max or Spark Flex to manage.
    */
   public static void register(CANSparkBase spark) {
-    faultReporters.add(
-        () -> {
-          for (FaultID fault : FaultID.values()) {
-            if (spark.getFault(fault)) {
-              report(SparkUtils.name(spark), fault.name(), FaultType.ERROR);
-            }
-          }
-        });
+    for (FaultID fault : FaultID.values()) {
+      register(() -> spark.getFault(fault), SparkUtils.name(spark), fault.name(), FaultType.ERROR);
+    }
     register(
         () -> spark.getMotorTemperature() > 100,
         SparkUtils.name(spark),
@@ -234,18 +219,20 @@ public static void register(AHRS ahrs) {
    */
   public static void register(PowerDistribution powerDistribution) {
     var fields = PowerDistributionFaults.class.getFields();
-    faultReporters.add(
-        () -> {
-          try {
-            PowerDistributionFaults faults = powerDistribution.getFaults();
-            for (Field fault : fields) {
-              if (fault.getBoolean(faults)) {
-                report("Power Distribution", fault.getName(), FaultType.ERROR);
+    for (Field fault : fields) {
+      register(
+          () -> {
+            try {
+              if (fault.getBoolean(powerDistribution.getFaults())) {
+                return Optional.of(
+                    new Fault("Power Distribution", fault.getName(), FaultType.ERROR));
               }
+            } catch (Exception e) {
             }
-          } catch (Exception e) {
-          }
-        });
+            return Optional.empty();
+          });
+    }
+    ;
   }
 
   /**

src/main/java/org/sciborgs1155/lib/InputStream.java

@@ -30,7 +30,7 @@ public static InputStream hypot(InputStream x, InputStream y) {
   }
 
   public static InputStream atan(InputStream x, InputStream y) {
-    return () -> Math.atan2(x.get(), y.get());
+    return () -> Math.atan2(y.get(), x.get());
   }
 
   /**
@@ -97,8 +97,8 @@ public default InputStream add(DoubleSupplier offset) {
    * @param factor An offset.
    * @return An offset stream.
    */
-  public default InputStream add(double factor) {
-    return add(() -> factor);
+  public default InputStream add(double offset) {
+    return add(() -> offset);
   }
 
   /**
@@ -159,7 +159,7 @@ public default InputStream clamp(double magnitude) {
    * @return A rate limited stream.
    */
   public default InputStream rateLimit(double rate) {
-    var limiter = new SlewRateLimiter(rate);
+    var limiter = new SlewRateLimiter(rate, -rate, get());
     return map(x -> limiter.calculate(x));
   }
 

src/main/java/org/sciborgs1155/robot/Robot.java

@@ -93,8 +93,9 @@ private void configureGameBehavior() {
 
   /** Configures trigger -> command bindings. */
   private void configureBindings() {
-    InputStream x = InputStream.of(driver::getLeftX).negate();
-    InputStream y = InputStream.of(driver::getLeftY).negate();
+    // x and y are switched: we use joystick Y axis to control field x motion
+    InputStream x = InputStream.of(driver::getLeftY).negate();
+    InputStream y = InputStream.of(driver::getLeftX).negate();
 
     // Apply speed multiplier, deadband, square inputs, and scale translation to max speed
     InputStream r =

src/main/java/org/sciborgs1155/robot/commands/Autos.java

@@ -28,7 +28,7 @@ public static SendableChooser<Command> configureAutos(Drive drive) {
     AutoBuilder.configureHolonomic(
         drive::pose,
         drive::resetOdometry,
-        drive::getRobotRelativeChassisSpeeds,
+        drive::robotRelativeChassisSpeeds,
         s -> drive.setChassisSpeeds(s, ControlMode.CLOSED_LOOP_VELOCITY),
         new HolonomicPathFollowerConfig(
             new PIDConstants(Translation.P, Translation.I, Translation.D),

src/main/java/org/sciborgs1155/robot/drive/Drive.java

@@ -401,14 +401,14 @@ public SwerveModulePosition[] modulePositions() {
 
   /** Returns the robot-relative chassis speeds. */
   @Log.NT
-  public ChassisSpeeds getRobotRelativeChassisSpeeds() {
+  public ChassisSpeeds robotRelativeChassisSpeeds() {
     return kinematics.toChassisSpeeds(moduleStates());
   }
 
   /** Returns the field-relative chassis speeds. */
   @Log.NT
-  public ChassisSpeeds getFieldRelativeChassisSpeeds() {
-    return ChassisSpeeds.fromRobotRelativeSpeeds(getRobotRelativeChassisSpeeds(), heading());
+  public ChassisSpeeds fieldRelativeChassisSpeeds() {
+    return ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeChassisSpeeds(), heading());
   }
 
   /**
@@ -457,8 +457,7 @@ public void simulationPeriodic() {
     simRotation =
         simRotation.rotateBy(
             Rotation2d.fromRadians(
-                getRobotRelativeChassisSpeeds().omegaRadiansPerSecond
-                    * Constants.PERIOD.in(Seconds)));
+                robotRelativeChassisSpeeds().omegaRadiansPerSecond * Constants.PERIOD.in(Seconds)));
   }
 
   /** Stops the drivetrain. */

src/test/java/org/sciborgs1155/lib/FaultLoggerTest.java

@@ -33,23 +33,23 @@ void report() {
         base.getSubTable("Total Faults").getStringArrayTopic("errors").subscribe(new String[10]);
     FaultLogger.update();
     FaultLogger.report("Test", "Example", FaultType.INFO);
-    FaultLogger.update();
     assertEquals(1, FaultLogger.activeFaults().size());
+    FaultLogger.update();
     assertEquals(1, FaultLogger.totalFaults().size());
     assertEquals(1, activeInfos.get().length);
     assertEquals(0, totalErrors.get().length);
 
     // duplicate
     FaultLogger.report("Test", "Example", FaultType.INFO);
-    FaultLogger.update();
     assertEquals(1, FaultLogger.activeFaults().size());
+    FaultLogger.update();
     assertEquals(1, FaultLogger.totalFaults().size());
     assertEquals(1, activeInfos.get().length);
     assertEquals(0, totalErrors.get().length);
 
     FaultLogger.report("Test2", "Example2", FaultType.ERROR);
-    FaultLogger.update();
     assertEquals(1, FaultLogger.activeFaults().size());
+    FaultLogger.update();
     assertEquals(2, FaultLogger.totalFaults().size());
     assertEquals(0, activeInfos.get().length);
     assertEquals(1, totalErrors.get().length);

src/test/java/org/sciborgs1155/lib/InputStreamTest.java

@@ -1,41 +1,81 @@
 package org.sciborgs1155.lib;
 
+import static edu.wpi.first.units.Units.Seconds;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.sciborgs1155.lib.InputStream.*;
 
+import edu.wpi.first.math.MathSharedStore;
 import org.junit.jupiter.api.Test;
 
 public class InputStreamTest {
 
-  private double two() {
-    return 2.0;
+  private InputStream stream(double n) {
+    return of(() -> n);
+  }
+
+  @Test
+  void hypotenuse() {
+    assertEquals(5, hypot(stream(3), stream(4)).get());
+  }
+
+  @Test
+  void arctan() {
+    assertEquals(-Math.PI / 4, atan(stream(1), stream(-1)).get());
+  }
+
+  @Test
+  void map() {
+    assertEquals(8, stream(3).map(n -> 40 * n % 16).get());
+  }
+
+  @Test
+  void scale() {
+    assertEquals(-12.44, stream(4).scale(() -> -3.11).get());
+    assertEquals(-12.44, stream(4).scale(-3.11).get());
+  }
+
+  @Test
+  void negate() {
+    assertEquals(-13.123, stream(13.123).negate().get());
+  }
+
+  @Test
+  void add() {
+    assertEquals(9, stream(7).add(2).get());
+    assertEquals(5.232, stream(1.232).add(() -> 4).get());
+  }
+
+  @Test
+  void pow() {
+    assertEquals(8, stream(2).pow(3).get());
   }
 
   @Test
   void signedPow() {
-    var stream = InputStream.of(this::two).negate().clamp(1.8).signedPow(2);
-    System.out.println(stream.get());
-    assert stream.get() == -3.24;
+    assertEquals(0.25, stream(0.5).signedPow(2).get());
+    assertEquals(-0.25, stream(-0.5).signedPow(2).get());
   }
 
   @Test
   void deadband() {
-    var stream = InputStream.of(this::two).negate().scale(0.25);
-    assertEquals(stream.deadband(0.6, 1).get(), 0.0);
-    assertEquals(stream.deadband(0.1, 1).get(), -0.444444, 0.01);
+    assertEquals(0, stream(-0.5).deadband(0.6, 1).get());
+    assertEquals(2.25, stream(2).deadband(0.2, 1).get());
   }
 
   @Test
-  void rateLimit() {
-    InputStream cursed =
-        new InputStream() {
-          double state = 0;
-
-          @Override
-          public double getAsDouble() {
-            return state += 2;
-          }
-        }.rateLimit(1);
+  void clamp() {
+    assertEquals(5.32, stream(5.32).clamp(6.5).get());
+    assertEquals(-6.5, stream(-7).clamp(6.5).get());
+  }
 
-    assertEquals(cursed.get(), 0, 0.1); // 0 time passes and we can't mock time
+  @Test
+  void rateLimit() {
+    UnitTestingUtil.setupTests();
+    InputStream stream = of(() -> MathSharedStore.getTimestamp() * 2);
+    InputStream limited = stream.rateLimit(1);
+    double initial = limited.get();
+    UnitTestingUtil.fastForward(Seconds.of(2));
+    assertEquals(4, stream.get() - initial, 0.1);
+    assertEquals(2, limited.get() - initial, 0.1);
   }
 }

src/test/java/org/sciborgs1155/robot/SwerveTest.java

@@ -62,7 +62,7 @@ public void reachesRobotVelocity() {
     run(drive.drive(() -> xVelocitySetpoint, () -> yVelocitySetpoint, drive::heading));
     fastForward(500);
 
-    ChassisSpeeds chassisSpeed = drive.getFieldRelativeChassisSpeeds();
+    ChassisSpeeds chassisSpeed = drive.fieldRelativeChassisSpeeds();
 
     assertEquals(xVelocitySetpoint, chassisSpeed.vxMetersPerSecond, DELTA);
     assertEquals(yVelocitySetpoint, chassisSpeed.vyMetersPerSecond, DELTA);
@@ -79,7 +79,7 @@ public void reachesAngularVelocity() {
                     ControlMode.CLOSED_LOOP_VELOCITY)));
     fastForward();
 
-    ChassisSpeeds chassisSpeed = drive.getRobotRelativeChassisSpeeds();
+    ChassisSpeeds chassisSpeed = drive.robotRelativeChassisSpeeds();
     assertEquals(omegaRadiansPerSecond, chassisSpeed.omegaRadiansPerSecond, DELTA);
   }