Add PeriodicRegistry

Author: kcooney

lib/src/main/java/com/team2813/lib2813/subsystems/ModularSubsystem.java

@@ -0,0 +1,56 @@
+package com.team2813.lib2813.subsystems;
+
+import com.team2813.lib2813.util.PeriodicRegistry;
+import com.team2813.lib2813.util.SimplePeriodicRegistry;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * A base for subsystems that has APIs to support modular code.
+ *
+ * <p>Provides access to {@link PeriodicRegistry}, which can be passed to reusable components.
+ */
+public abstract class ModularSubsystem extends SubsystemBase {
+  private final SimplePeriodicRegistry periodicRegistry = new SimplePeriodicRegistry();
+
+  /** Constructor. Telemetry/log name defaults to the classname. */
+  public ModularSubsystem() {
+    super();
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param name Name of the subsystem for telemetry and logging.
+   */
+  protected ModularSubsystem(String name) {
+    super(name);
+  }
+
+  /**
+   * Gets a periodic registry for registering functions to periodically be called by this subsystem.
+   */
+  protected final PeriodicRegistry getPeriodicRegistry() {
+    return periodicRegistry;
+  }
+
+  /**
+   * Called periodically by the {@link CommandScheduler}.
+   *
+   * <p>This is intentionally marked as final; subclasses should use {@link #getPeriodicRegistry()}.
+   */
+  @Override
+  public final void periodic() {
+    periodicRegistry.callPeriodicFunctions();
+  }
+
+  /**
+   * Called periodically by the {@link CommandScheduler} when in simulation mode.
+   *
+   * <p>This is intentionally marked as final; subclasses should use {@link #getPeriodicRegistry()}.
+   */
+  @Override
+  public final void simulationPeriodic() {
+    periodicRegistry.callSimulationPeriodicFunctions();
+  }
+}

lib/src/main/java/com/team2813/lib2813/util/PeriodicRegistry.java

@@ -0,0 +1,27 @@
+package com.team2813.lib2813.util;
+
+import java.util.function.Consumer;
+
+/**
+ * Service for registering callbacks that need to run periodically.
+ *
+ * <p>>Subsystems can extend {@link com.team2813.lib2813.subsystems.ModularSubsystem} to get access
+ * to an instance of {@link PeriodicRegistry}. This will ensure that time spent in the periodic
+ * methods are associated with the subsystem.
+ */
+public interface PeriodicRegistry {
+
+  /**
+   * Add a callback to run for each iteration of the event loop.
+   *
+   * @param callback The callback to run.
+   */
+  void addPeriodic(Consumer<RobotState> callback);
+
+  /**
+   * Add a callback to run in simulation mode for each iteration of the event loop.
+   *
+   * @param callback The callback to run.
+   */
+  void addSimulationPeriodic(Consumer<RobotState> callback);
+}

lib/src/main/java/com/team2813/lib2813/util/RobotState.java

@@ -0,0 +1,43 @@
+package com.team2813.lib2813.util;
+
+/** Provides APIs for getting the current state of the robot. */
+public interface RobotState {
+
+  /**
+   * Determines if the Robot is currently enabled.
+   *
+   * @return {@code True} if the Robot is currently enabled by the Driver Station.
+   */
+  boolean isEnabled();
+
+  /**
+   * Determines if the Robot is currently disabled.
+   *
+   * @return {@code True} if the Robot is currently disabled by the Driver Station.
+   */
+  default boolean isDisabled() {
+    return !isEnabled();
+  }
+
+  /**
+   * Determines if the robot is currently in Autonomous mode as determined by the Driver Station.
+   *
+   * @return {@code True} if the robot is currently operating autonomously.
+   */
+  boolean isAutonomous();
+
+  /**
+   * Determine sif the robot is currently in Test mode as determined by the Driver Station.
+   *
+   * @return {@code True} if the robot is currently operating in Test mode.
+   */
+  boolean isTest();
+
+  /**
+   * Determine if the robot is currently in Operator Control mode as determined by the Driver
+   * Station.
+   *
+   * @return {@code True} if the robot is currently operating in Operator Control mode.
+   */
+  boolean isTeleop();
+}

lib/src/main/java/com/team2813/lib2813/util/SimplePeriodicRegistry.java

@@ -0,0 +1,72 @@
+package com.team2813.lib2813.util;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.function.Consumer;
+
+/**
+ * {@link PeriodicRegistry} implementation that gets the state from {@link DriverStation}.
+ *
+ * <p>Most code should not directly use this class. Subsystems can extend {@link
+ * com.team2813.lib2813.subsystems.ModularSubsystem} to get access to an instance of {@link
+ * PeriodicRegistry}.
+ *
+ * <p>Alternatively, the main robot code can construct this class, as long as it calls {@link
+ * #callPeriodicFunctions()} in {@code robotPeriodic()} and {@link
+ * #callSimulationPeriodicFunctions()} in {@code simulationPeriodic()}.
+ */
+public final class SimplePeriodicRegistry implements PeriodicRegistry {
+  private final List<Consumer<RobotState>> periodicFunctions = new ArrayList<>();
+  private final List<Consumer<RobotState>> simulationPeriodicFunctions = new ArrayList<>();
+
+  @Override
+  public void addPeriodic(Consumer<RobotState> callback) {
+    periodicFunctions.add(callback);
+  }
+
+  @Override
+  public void addSimulationPeriodic(Consumer<RobotState> callback) {
+    simulationPeriodicFunctions.add(callback);
+  }
+
+  /** Calls all the callbacks added via {@link #addPeriodic(Consumer)}. */
+  public void callPeriodicFunctions() {
+    RobotState robotState = SimpleRobotState.getInstance();
+    periodicFunctions.forEach(fun -> fun.accept(robotState));
+  }
+
+  /** Calls all the callbacks added via {@link #addSimulationPeriodic(Consumer)}. */
+  public void callSimulationPeriodicFunctions() {
+    RobotState robotState = SimpleRobotState.getInstance();
+    simulationPeriodicFunctions.forEach(fun -> fun.accept(robotState));
+  }
+
+  private static class SimpleRobotState implements RobotState {
+    private static final SimpleRobotState INSTANCE = new SimpleRobotState();
+
+    static RobotState getInstance() {
+      return INSTANCE;
+    }
+
+    @Override
+    public boolean isEnabled() {
+      return DriverStation.isEnabled();
+    }
+
+    @Override
+    public boolean isAutonomous() {
+      return DriverStation.isAutonomous();
+    }
+
+    @Override
+    public boolean isTest() {
+      return DriverStation.isTest();
+    }
+
+    @Override
+    public boolean isTeleop() {
+      return DriverStation.isTeleop();
+    }
+  }
+}