From 87a7cd420e76a1405d3ac3e6e345249753636c85 Mon Sep 17 00:00:00 2001 From: sigalrmp Date: Sat, 14 Dec 2024 13:58:33 -0500 Subject: [PATCH 1/4] removed gets --- src/main/java/org/sciborgs1155/robot/commands/Autos.java | 2 +- src/main/java/org/sciborgs1155/robot/drive/Drive.java | 9 ++++----- src/test/java/org/sciborgs1155/robot/SwerveTest.java | 4 ++-- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/sciborgs1155/robot/commands/Autos.java b/src/main/java/org/sciborgs1155/robot/commands/Autos.java index a134332..c31453e 100644 --- a/src/main/java/org/sciborgs1155/robot/commands/Autos.java +++ b/src/main/java/org/sciborgs1155/robot/commands/Autos.java @@ -28,7 +28,7 @@ public static SendableChooser 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), diff --git a/src/main/java/org/sciborgs1155/robot/drive/Drive.java b/src/main/java/org/sciborgs1155/robot/drive/Drive.java index a0357e2..b8f3ea1 100644 --- a/src/main/java/org/sciborgs1155/robot/drive/Drive.java +++ b/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. */ diff --git a/src/test/java/org/sciborgs1155/robot/SwerveTest.java b/src/test/java/org/sciborgs1155/robot/SwerveTest.java index 50499da..b41c0f8 100644 --- a/src/test/java/org/sciborgs1155/robot/SwerveTest.java +++ b/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); } From d1990f8ae29072c6a0ee47f170e0151bf1ae4b66 Mon Sep 17 00:00:00 2001 From: sigalrmp Date: Sat, 14 Dec 2024 14:11:42 -0500 Subject: [PATCH 2/4] input stream fixes and testing --- .../org/sciborgs1155/lib/InputStream.java | 8 +- .../java/org/sciborgs1155/robot/Robot.java | 5 +- .../org/sciborgs1155/lib/InputStreamTest.java | 78 ++++++++++++++----- 3 files changed, 66 insertions(+), 25 deletions(-) diff --git a/src/main/java/org/sciborgs1155/lib/InputStream.java b/src/main/java/org/sciborgs1155/lib/InputStream.java index 3ac9b44..79bbe11 100644 --- a/src/main/java/org/sciborgs1155/lib/InputStream.java +++ b/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)); } diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index a4baf31..e78db04 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/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 = diff --git a/src/test/java/org/sciborgs1155/lib/InputStreamTest.java b/src/test/java/org/sciborgs1155/lib/InputStreamTest.java index e8ba4ac..822e8e3 100644 --- a/src/test/java/org/sciborgs1155/lib/InputStreamTest.java +++ b/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 hypotenous() { + 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); } } From 44533b3122f0a86b582fb9f14db0cd2cd204f0f6 Mon Sep 17 00:00:00 2001 From: sigalrmp Date: Sat, 14 Dec 2024 14:35:39 -0500 Subject: [PATCH 3/4] fault logger cleanup --- .../org/sciborgs1155/lib/FaultLogger.java | 65 ++++++++----------- .../org/sciborgs1155/lib/FaultLoggerTest.java | 6 +- 2 files changed, 29 insertions(+), 42 deletions(-) diff --git a/src/main/java/org/sciborgs1155/lib/FaultLogger.java b/src/main/java/org/sciborgs1155/lib/FaultLogger.java index 29c690c..899ab50 100644 --- a/src/main/java/org/sciborgs1155/lib/FaultLogger.java +++ b/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 faults) { } // DATA - private static final List faultReporters = new ArrayList<>(); - private static final Set newFaults = new HashSet<>(); + private static final List>> faultReporters = new ArrayList<>(); private static final Set activeFaults = new HashSet<>(); private static final Set totalFaults = new HashSet<>(); @@ -91,23 +85,20 @@ public void set(Set 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 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> supplier) { - faultReporters.add(() -> supplier.get().ifPresent(FaultLogger::report)); + faultReporters.add(supplier); } /** @@ -176,12 +167,11 @@ public static void register(Supplier> 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(); + }); + } + ; } /** diff --git a/src/test/java/org/sciborgs1155/lib/FaultLoggerTest.java b/src/test/java/org/sciborgs1155/lib/FaultLoggerTest.java index a2f3bf9..019015b 100644 --- a/src/test/java/org/sciborgs1155/lib/FaultLoggerTest.java +++ b/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); From 98cf80af2a174a9dafcf553ce0bda3b3dfa95fcd Mon Sep 17 00:00:00 2001 From: sigalrmp Date: Sun, 15 Dec 2024 21:52:18 -0500 Subject: [PATCH 4/4] fixed type --- src/test/java/org/sciborgs1155/lib/InputStreamTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test/java/org/sciborgs1155/lib/InputStreamTest.java b/src/test/java/org/sciborgs1155/lib/InputStreamTest.java index 822e8e3..05d8ee0 100644 --- a/src/test/java/org/sciborgs1155/lib/InputStreamTest.java +++ b/src/test/java/org/sciborgs1155/lib/InputStreamTest.java @@ -14,7 +14,7 @@ private InputStream stream(double n) { } @Test - void hypotenous() { + void hypotenuse() { assertEquals(5, hypot(stream(3), stream(4)).get()); }