From c2632d0b06a975232f2409040fafa777f9f3ea54 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Mon, 20 Mar 2023 13:50:27 -0700 Subject: [PATCH 1/3] make lib199 periodic methods run asynchronously --- .../lib199/Lib199Subsystem.java | 43 ++++++++++++++++++- .../org/carlmontrobotics/lib199/Mocks.java | 4 +- .../carlmontrobotics/lib199/MotorErrors.java | 2 +- .../lib199/sim/MockedCANCoder.java | 2 +- .../lib199/sim/MockedSparkEncoder.java | 2 +- .../lib199/Lib199SubsystemTest.java | 2 +- .../lib199/MotorErrorsTest.java | 35 ++++++++++++--- 7 files changed, 77 insertions(+), 13 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java index 5636cc2e..0f9e5880 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java +++ b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java @@ -1,8 +1,10 @@ package org.carlmontrobotics.lib199; import java.util.ArrayList; +import java.util.concurrent.CopyOnWriteArrayList; import java.util.function.Consumer; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Subsystem; public class Lib199Subsystem implements Subsystem { @@ -10,12 +12,29 @@ public class Lib199Subsystem implements Subsystem { private static final Lib199Subsystem INSTANCE = new Lib199Subsystem(); private static final ArrayList periodicMethods = new ArrayList<>(); private static final ArrayList periodicSimulationMethods = new ArrayList<>(); + private static final CopyOnWriteArrayList asyncPeriodicMethods = new CopyOnWriteArrayList<>(); + private static final CopyOnWriteArrayList asyncPeriodicSimulationMethods = new CopyOnWriteArrayList<>(); private static final Consumer RUN_RUNNABLE = Runnable::run; + private static final Thread asyncPeriodicThread; + + public static final long asyncSleepTime = 20; + static { ensureRegistered(); + + asyncPeriodicThread = new Thread(() -> { + while(true) { + INSTANCE.asyncPeriodic(); + try { + Thread.sleep(asyncSleepTime); + } catch(InterruptedException e) {} + } + }); + asyncPeriodicThread.setDaemon(true); + asyncPeriodicThread.start(); } - + private static boolean registered = false; private static void ensureRegistered() { @@ -30,10 +49,27 @@ public static void registerPeriodic(Runnable method) { periodicMethods.add(method); } + @Deprecated + /** + * @deprecated Use registerSimulationPeriodic + * @param method + */ public static void simulationPeriodic(Runnable method) { + registerSimulationPeriodic(method); + } + + public static void registerSimulationPeriodic(Runnable method) { periodicSimulationMethods.add(method); } + public static void registerAsyncPeriodic(Runnable method) { + asyncPeriodicMethods.add(method); + } + + public static void registerAsyncSimulationPeriodic(Runnable method) { + if(RobotBase.isSimulation()) asyncPeriodicSimulationMethods.add(method); + } + @Override public void periodic() { periodicMethods.forEach(RUN_RUNNABLE); @@ -44,6 +80,11 @@ public void simulationPeriodic() { periodicSimulationMethods.forEach(RUN_RUNNABLE); } + public void asyncPeriodic() { + asyncPeriodicMethods.forEach(RUN_RUNNABLE); + asyncPeriodicSimulationMethods.forEach(RUN_RUNNABLE); + } + private Lib199Subsystem() {} } diff --git a/src/main/java/org/carlmontrobotics/lib199/Mocks.java b/src/main/java/org/carlmontrobotics/lib199/Mocks.java index 9bc78694..da97d787 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Mocks.java +++ b/src/main/java/org/carlmontrobotics/lib199/Mocks.java @@ -24,7 +24,7 @@ public final class Mocks { private static final List> MOCKS = Collections.synchronizedList(new ArrayList<>()); private static final Predicate> IS_REFERENCE_CLEARED = reference -> reference.get() == null; private static final Consumer> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK = reference -> Mockito.clearInvocations(reference.get()); - private static final Predicate> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERNCE_NOT_CLEARED = reference -> { + private static final Predicate> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED = reference -> { if(IS_REFERENCE_CLEARED.test(reference)) return true; CLEAR_INVOCATIONS_ON_REFERENCED_MOCK.accept(reference); return false; @@ -37,7 +37,7 @@ public final class Mocks { // 2) Garbage collected references are removed // 3) Mock is garbage collected // 4) Mock invocations are cleared -> throws NullPointerException - Lib199Subsystem.registerPeriodic(() -> MOCKS.removeIf(CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERNCE_NOT_CLEARED)); + Lib199Subsystem.registerAsyncPeriodic(() -> MOCKS.removeIf(CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED)); } /** diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java index 981669dd..b5bb7b6d 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java @@ -20,7 +20,7 @@ public final class MotorErrors { private static final HashMap stickyFlags = new HashMap<>(); static { - Lib199Subsystem.registerPeriodic(MotorErrors::doReportSparkMaxTemp); + Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkMaxTemp); } public static void reportError(ErrorCode error) { diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java index 534a9c9b..90d1856c 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java @@ -29,7 +29,7 @@ public MockedCANCoder(CANCoder canCoder) { position = device.createDouble("count", Direction.kInput, 0); gearing = device.createDouble("gearing", Direction.kOutput, 1); sim = canCoder.getSimCollection(); - Lib199Subsystem.registerPeriodic(this::update); + Lib199Subsystem.registerAsyncSimulationPeriodic(this::update); sims.put(port, this); } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java index ae809ca5..68ff907f 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java @@ -30,7 +30,7 @@ public MockedSparkEncoder(int id) { count = device.createDouble("count", Direction.kInput, 0); gearing = device.createDouble("gearing", Direction.kOutput, 1); sims.put(id, this); - Lib199Subsystem.registerPeriodic(this); + Lib199Subsystem.registerAsyncPeriodic(this); } public double getPosition() { diff --git a/src/test/java/org/carlmontrobotics/lib199/Lib199SubsystemTest.java b/src/test/java/org/carlmontrobotics/lib199/Lib199SubsystemTest.java index 81ed1ea4..240aeff7 100644 --- a/src/test/java/org/carlmontrobotics/lib199/Lib199SubsystemTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/Lib199SubsystemTest.java @@ -25,7 +25,7 @@ public void testPeriodic() { public void testSimulationPeriodic() { assumeTrue(RobotBase.isSimulation()); AtomicInteger counter = new AtomicInteger(0); - Lib199Subsystem.registerPeriodic(() -> counter.addAndGet(1)); + Lib199Subsystem.registerSimulationPeriodic(() -> counter.addAndGet(1)); assertEquals("Simulation periodic method called before CommandScheduler.run", 0, counter.get()); CommandScheduler.getInstance().run(); assertEquals("Simulation periodic method called more than once or not at all", 1, counter.get()); diff --git a/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java b/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java index dde70623..a6ad4519 100644 --- a/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java @@ -2,6 +2,7 @@ import static org.junit.Assert.assertEquals; import static org.junit.Assert.assertNotEquals; +import static org.junit.Assume.assumeNoException; import com.ctre.phoenix.ErrorCode; import com.revrobotics.REVLibError; @@ -12,7 +13,6 @@ import org.junit.Test; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandScheduler; public class MotorErrorsTest extends ErrStreamTest { @@ -77,6 +77,16 @@ public int getDeviceId() { } + private static final Object asyncPeriodicNotifier = new Object(); + + static { + Lib199Subsystem.registerAsyncPeriodic(() -> { + synchronized(asyncPeriodicNotifier) { + asyncPeriodicNotifier.notifyAll(); + } + }); + } + @Test public void testOkErrors() { errStream.reset(); @@ -165,30 +175,43 @@ private void doTestReportSparkMaxTemp(int id) { MotorErrors.reportSparkMaxTemp((CANSparkMax)spark, 40); spark.setSmartCurrentLimit(50); spark.setTemperature(20); - CommandScheduler.getInstance().run(); + runAsyncPeriodic(); String smartDashboardKey = "Port " + id + " Spark Max Temp"; assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); spark.setTemperature(20); - CommandScheduler.getInstance().run(); + runAsyncPeriodic(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); assertEquals(0, errStream.size()); spark.setTemperature(40); - CommandScheduler.getInstance().run(); + runAsyncPeriodic(); assertEquals(40, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(1, spark.getSmartCurrentLimit()); assertNotEquals(0, errStream.size()); errStream.reset(); spark.setTemperature(50); - CommandScheduler.getInstance().run(); + runAsyncPeriodic(); assertEquals(50, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(1, spark.getSmartCurrentLimit()); spark.setTemperature(20); - CommandScheduler.getInstance().run(); + runAsyncPeriodic(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(1, spark.getSmartCurrentLimit()); assertEquals(0, errStream.size()); } + // Ensures an update to the asynchronous periodic thread is run + private void runAsyncPeriodic() { + try { + synchronized(asyncPeriodicNotifier) { + // Run twice because we don't know in what order we're called, so make sure all periodic methods are run twice + asyncPeriodicNotifier.wait(); + asyncPeriodicNotifier.wait(); + } + } catch(InterruptedException e) { + assumeNoException(e); + } + } + } From 225a9ed339eb01347881a80dd3227272be59bb89 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Sat, 8 Apr 2023 14:08:41 -0700 Subject: [PATCH 2/3] ensure all collections are concurrent --- .../lib199/Lib199Subsystem.java | 5 ++-- .../org/carlmontrobotics/lib199/Mocks.java | 5 ++-- .../carlmontrobotics/lib199/MotorErrors.java | 14 +++++------ .../lib199/sim/MockPhoenixController.java | 8 +++---- .../lib199/sim/MockSparkMax.java | 24 +++++++++---------- 5 files changed, 27 insertions(+), 29 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java index 0f9e5880..ad9beaf9 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java +++ b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java @@ -1,6 +1,5 @@ package org.carlmontrobotics.lib199; -import java.util.ArrayList; import java.util.concurrent.CopyOnWriteArrayList; import java.util.function.Consumer; @@ -10,8 +9,8 @@ public class Lib199Subsystem implements Subsystem { private static final Lib199Subsystem INSTANCE = new Lib199Subsystem(); - private static final ArrayList periodicMethods = new ArrayList<>(); - private static final ArrayList periodicSimulationMethods = new ArrayList<>(); + private static final CopyOnWriteArrayList periodicMethods = new CopyOnWriteArrayList<>(); + private static final CopyOnWriteArrayList periodicSimulationMethods = new CopyOnWriteArrayList<>(); private static final CopyOnWriteArrayList asyncPeriodicMethods = new CopyOnWriteArrayList<>(); private static final CopyOnWriteArrayList asyncPeriodicSimulationMethods = new CopyOnWriteArrayList<>(); private static final Consumer RUN_RUNNABLE = Runnable::run; diff --git a/src/main/java/org/carlmontrobotics/lib199/Mocks.java b/src/main/java/org/carlmontrobotics/lib199/Mocks.java index da97d787..54c44357 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Mocks.java +++ b/src/main/java/org/carlmontrobotics/lib199/Mocks.java @@ -6,9 +6,8 @@ import java.lang.reflect.Modifier; import java.util.ArrayList; import java.util.Arrays; -import java.util.Collections; import java.util.HashMap; -import java.util.List; +import java.util.concurrent.CopyOnWriteArrayList; import java.util.function.Consumer; import java.util.function.Predicate; import java.util.stream.Collectors; @@ -21,7 +20,7 @@ public final class Mocks { - private static final List> MOCKS = Collections.synchronizedList(new ArrayList<>()); + private static final CopyOnWriteArrayList> MOCKS = new CopyOnWriteArrayList<>(); private static final Predicate> IS_REFERENCE_CLEARED = reference -> reference.get() == null; private static final Consumer> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK = reference -> Mockito.clearInvocations(reference.get()); private static final Predicate> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED = reference -> { diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java index b5bb7b6d..0955326b 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java @@ -1,8 +1,8 @@ package org.carlmontrobotics.lib199; -import java.util.ArrayList; import java.util.Arrays; -import java.util.HashMap; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.CopyOnWriteArrayList; import com.ctre.phoenix.ErrorCode; import com.revrobotics.CANSparkMax; @@ -13,11 +13,11 @@ public final class MotorErrors { - private static final HashMap temperatureSparks = new HashMap<>(); - private static final HashMap sparkTemperatureLimits = new HashMap<>(); - private static final ArrayList overheatedSparks = new ArrayList<>(); - private static final HashMap flags = new HashMap<>(); - private static final HashMap stickyFlags = new HashMap<>(); + private static final ConcurrentHashMap temperatureSparks = new ConcurrentHashMap<>(); + private static final ConcurrentHashMap sparkTemperatureLimits = new ConcurrentHashMap<>(); + private static final CopyOnWriteArrayList overheatedSparks = new CopyOnWriteArrayList<>(); + private static final ConcurrentHashMap flags = new ConcurrentHashMap<>(); + private static final ConcurrentHashMap stickyFlags = new ConcurrentHashMap<>(); static { Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkMaxTemp); diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java index 2af20458..af092a76 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java @@ -1,7 +1,7 @@ package org.carlmontrobotics.lib199.sim; -import java.util.ArrayList; -import java.util.HashMap; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.CopyOnWriteArrayList; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.IMotorController; @@ -15,7 +15,7 @@ abstract class MockPhoenixController implements AutoCloseable { // CAN ports should be separate from PWM ports protected PWMMotorController motorPWM; // Since we need to keep a record of all the motor's followers - protected static HashMap> followMap = new HashMap<>(); + protected static ConcurrentHashMap> followMap = new ConcurrentHashMap<>(); public MockPhoenixController(int portPWM) { this.portPWM = portPWM; @@ -36,7 +36,7 @@ public double get() { public void follow(IMotorController leader) { if (!followMap.containsKey(leader.getDeviceID())) { - ArrayList arr = new ArrayList(); + CopyOnWriteArrayList arr = new CopyOnWriteArrayList(); arr.add(motorPWM); followMap.put(leader.getDeviceID(), arr); } else { diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 97db8ea1..d6185133 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -1,23 +1,23 @@ package org.carlmontrobotics.lib199.sim; -import java.util.ArrayList; -import java.util.HashMap; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.CopyOnWriteArrayList; + +import org.carlmontrobotics.lib199.DummySparkMaxAnswer; +import org.carlmontrobotics.lib199.Mocks; +import org.carlmontrobotics.lib199.REVLibErrorAnswer; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ExternalFollower; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import org.carlmontrobotics.lib199.DummySparkMaxAnswer; -import org.carlmontrobotics.lib199.Mocks; -import org.carlmontrobotics.lib199.REVLibErrorAnswer; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDouble; import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; public class MockSparkMax { // Assign the CAN port to a PWM port so it works with the simulator. Not a fan @@ -30,7 +30,7 @@ public class MockSparkMax { private SparkMaxPIDController pidController; private boolean isInverted; // Since we need to keep a record of all the motor's followers - private static HashMap> followMap = new HashMap<>(); + private static ConcurrentHashMap> followMap = new ConcurrentHashMap<>(); public MockSparkMax(int port, MotorType type) { this.port = port; @@ -67,7 +67,7 @@ public REVLibError follow(ExternalFollower leader, int deviceID) { public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) { if (!followMap.containsKey(deviceID)) { - followMap.put(deviceID, new ArrayList()); + followMap.put(deviceID, new CopyOnWriteArrayList()); } followMap.get(deviceID).add(speed); return REVLibError.kOk; From d365ddb945e97fed2665d5ac78cb27aad0195774 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Sun, 21 May 2023 01:06:45 -0700 Subject: [PATCH 3/3] fix possible edge-case If a mock's reference was cleared between IS_REFERENCE_CLEARED.test and CLEAR_INVOCATIONS_ON_REFERENCED_MOCK.accept, the latter would throw an exception. By storing the referenced value in a member variable throughout both operations, the value cannot be null. This change should be tested before being merged into master --- .../java/org/carlmontrobotics/lib199/Mocks.java | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/Mocks.java b/src/main/java/org/carlmontrobotics/lib199/Mocks.java index 54c44357..07f7a4da 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Mocks.java +++ b/src/main/java/org/carlmontrobotics/lib199/Mocks.java @@ -8,8 +8,6 @@ import java.util.Arrays; import java.util.HashMap; import java.util.concurrent.CopyOnWriteArrayList; -import java.util.function.Consumer; -import java.util.function.Predicate; import java.util.stream.Collectors; import org.mockito.MockSettings; @@ -21,13 +19,6 @@ public final class Mocks { private static final CopyOnWriteArrayList> MOCKS = new CopyOnWriteArrayList<>(); - private static final Predicate> IS_REFERENCE_CLEARED = reference -> reference.get() == null; - private static final Consumer> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK = reference -> Mockito.clearInvocations(reference.get()); - private static final Predicate> CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED = reference -> { - if(IS_REFERENCE_CLEARED.test(reference)) return true; - CLEAR_INVOCATIONS_ON_REFERENCED_MOCK.accept(reference); - return false; - }; static { // Use a single predicate so that clearing references and invocations is an atomic operation @@ -36,7 +27,12 @@ public final class Mocks { // 2) Garbage collected references are removed // 3) Mock is garbage collected // 4) Mock invocations are cleared -> throws NullPointerException - Lib199Subsystem.registerAsyncPeriodic(() -> MOCKS.removeIf(CLEAR_INVOCATIONS_ON_REFERENCED_MOCK_IF_REFERENCE_NOT_CLEARED)); + Lib199Subsystem.registerAsyncPeriodic(() -> MOCKS.removeIf(reference -> { + Object mock = reference.get(); + if(mock == null) return true; + Mockito.clearInvocations(mock); + return false; + })); } /**