Inject PeriodicRegistry

Author: kcooney

lib/src/main/java/com/team2813/lib2813/control/motors/PositionalMotor.java

@@ -4,6 +4,7 @@
 import com.team2813.lib2813.control.Encoder;
 import com.team2813.lib2813.control.Motor;
 import com.team2813.lib2813.control.PIDMotor;
+import com.team2813.lib2813.util.PeriodicRegistry;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.DoublePublisher;
@@ -18,8 +19,6 @@
 /**
  * A wrapper around a motor and a PID controller that supports predefined positions.
  *
- * <p>The creator is responsible for calling {@link #periodic()} periodically.
- *
  * <p>When the motor is created, PID control is disabled. To enable PID control, call {@link
  * #setSetpoint(P)}; the motor will moving toward the setpoint and maintains position at the
  * setpoint under the control of the PID controller. To stop the motor, call {@link #stopMotor()}.
@@ -46,33 +45,36 @@ public final class PositionalMotor<P extends Enum<P> & Supplier<Angle>> implemen
    *
    * <p>The default acceptable error is {@value #DEFAULT_ERROR} and the PID constants are set to 0.
    *
+   * @param periodicRegistry periodic registry that the motor should use
    * @param motor the motor to control
    * @param encoder the encoder providing feedback
    * @param initialPosition the initial position of the controller
    * @return builder instance
    */
   public static <P extends Enum<P> & Supplier<Angle>> Builder<P> builder(
-      Motor motor, Encoder encoder, P initialPosition) {
-    return new Builder<>(motor, encoder, initialPosition);
+      PeriodicRegistry periodicRegistry, Motor motor, Encoder encoder, P initialPosition) {
+    return new Builder<>(periodicRegistry, motor, encoder, initialPosition);
   }
 
   /**
    * Creates a new builder for a {@code PositionalMotor} using a motor that has a built-in encoder.
    *
    * <p>The default acceptable error is {@value #DEFAULT_ERROR} and the PID constants are set to 0.
    *
+   * @param periodicRegistry periodic registry that the motor should use
    * @param motor the integrated motor controller
    * @param initialPosition the initial position of the controller
    * @return builder instance
    */
   public static <P extends Enum<P> & Supplier<Angle>> Builder<P> builder(
-      PIDMotor motor, P initialPosition) {
-    return new Builder<>(motor, motor, initialPosition);
+      PeriodicRegistry periodicRegistry, PIDMotor motor, P initialPosition) {
+    return new Builder<>(periodicRegistry, motor, motor, initialPosition);
   }
 
   /** A builder for a {@code PositionalMotor}. */
   public static class Builder<P extends Enum<P> & Supplier<Angle>> {
 
+    private final PeriodicRegistry periodicRegistry;
     private final Motor motor;
     private final Encoder encoder;
     private final double initialPosition;
@@ -82,7 +84,10 @@ public static class Builder<P extends Enum<P> & Supplier<Angle>> {
     private Clamper clamper = value -> value;
     private NetworkTable networkTable;
 
-    private Builder(Motor motor, Encoder encoder, P initialPosition) {
+    private Builder(
+        PeriodicRegistry periodicRegistry, Motor motor, Encoder encoder, P initialPosition) {
+      this.periodicRegistry =
+          Objects.requireNonNull(periodicRegistry, "periodicRegistry should not be null");
       this.motor = Objects.requireNonNull(motor, "motor should not be null");
       this.encoder = Objects.requireNonNull(encoder, "encoder should not be null");
       Objects.requireNonNull(initialPosition, "initialPosition should not be null");
@@ -190,6 +195,8 @@ private PositionalMotor(Builder<P> builder) {
 
     // TODO: Should we update the position of the controller using controller.calculate()?
     // TODO: Should we update the position of the encoder using encoder.setPosition()?
+
+    builder.periodicRegistry.addPeriodic(robotState -> this.periodic());
   }
 
   /**
@@ -258,12 +265,8 @@ public boolean atPosition() {
     return Math.abs(getMeasurement() - controller.getSetpoint()) <= acceptableError;
   }
 
-  /**
-   * Applies the PID output to the motor if this motor is enabled.
-   *
-   * <p>Should be called from {@link edu.wpi.first.wpilibj2.command.Subsystem#periodic()}.
-   */
-  public void periodic() {
+  /** Applies the PID output to the motor if this motor is enabled. */
+  private void periodic() {
     if (isPIDControlEnabled) {
       double nextOutput = controller.calculate(getMeasurement());
       motor.set(controlMode, clamper.clampValue(nextOutput));