Change build() to take in the unit

Author: kcooney

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

@@ -48,12 +48,10 @@ public final class PositionalMotor<P extends Enum<P> & Supplier<Angle>> implemen
    * @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(
-      PeriodicRegistry periodicRegistry, Motor motor, Encoder encoder, P initialPosition) {
-    return new Builder<>(periodicRegistry, motor, encoder, initialPosition);
+  public static Builder builder(PeriodicRegistry periodicRegistry, Motor motor, Encoder encoder) {
+    return new Builder(periodicRegistry, motor, encoder);
   }
 
   /**
@@ -63,36 +61,30 @@ public static <P extends Enum<P> & Supplier<Angle>> Builder<P> builder(
    *
    * @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(
-      PeriodicRegistry periodicRegistry, PIDMotor motor, P initialPosition) {
-    return new Builder<>(periodicRegistry, motor, motor, initialPosition);
+  public static Builder builder(PeriodicRegistry periodicRegistry, PIDMotor motor) {
+    return new Builder(periodicRegistry, motor, motor);
   }
 
   /** A builder for a {@code PositionalMotor}. */
-  public static class Builder<P extends Enum<P> & Supplier<Angle>> {
+  public static class Builder {
 
     private final PeriodicRegistry periodicRegistry;
     private final Motor motor;
     private final Encoder encoder;
-    private double initialPosition;
     private ControlMode controlMode = ControlMode.DUTY_CYCLE;
     private AngleUnit rotationUnit = Units.Rotations;
     private PIDController controller;
     private double acceptableError = DEFAULT_ERROR;
     private Clamper clamper = value -> value;
     private NetworkTable networkTable;
 
-    private Builder(
-        PeriodicRegistry periodicRegistry, Motor motor, Encoder encoder, P initialPosition) {
+    private Builder(PeriodicRegistry periodicRegistry, Motor motor, Encoder encoder) {
       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");
-      this.initialPosition = initialPosition.get().in(rotationUnit);
       controller = new PIDController(0, 0, 0);
     }
 
@@ -102,7 +94,7 @@ private Builder(
      * @param controller The PID controller
      * @return {@code this} for chaining
      */
-    public Builder<P> controller(PIDController controller) {
+    public Builder controller(PIDController controller) {
       this.controller = Objects.requireNonNull(controller, "controller should not be null");
       return this;
     }
@@ -115,7 +107,7 @@ public Builder<P> controller(PIDController controller) {
      * @return {@code this} for chaining
      * @throws IllegalArgumentException If {@code controlMode} is for positional control
      */
-    public Builder<P> controlMode(ControlMode controlMode) {
+    public Builder controlMode(ControlMode controlMode) {
       if (controlMode.isPositionalControl()) {
         throw new IllegalArgumentException(
             String.format(
@@ -135,7 +127,7 @@ public Builder<P> controlMode(ControlMode controlMode) {
      * @param d the derivative
      * @return {@code this} for chaining
      */
-    public Builder<P> PID(double p, double i, double d) {
+    public Builder PID(double p, double i, double d) {
       controller.setPID(p, i, d);
       return this;
     }
@@ -145,7 +137,7 @@ public Builder<P> PID(double p, double i, double d) {
      *
      * @return {@code this} for chaining
      */
-    public Builder<P> acceptableError(double error) {
+    public Builder acceptableError(double error) {
       this.acceptableError = error;
       return this;
     }
@@ -156,7 +148,7 @@ public Builder<P> acceptableError(double error) {
      * @param clamper Function to use to clamp output values
      * @return {@code this} for chaining
      */
-    public Builder<P> clamper(Clamper clamper) {
+    public Builder clamper(Clamper clamper) {
       this.clamper = Objects.requireNonNull(clamper, "clamper should not be null");
       return this;
     }
@@ -166,8 +158,7 @@ public Builder<P> clamper(Clamper clamper) {
      *
      * @param rotationUnit The angle unit to use for calculations
      */
-    public Builder<P> rotationUnit(AngleUnit rotationUnit) {
-      initialPosition = rotationUnit.convertFrom(initialPosition, this.rotationUnit);
+    public Builder rotationUnit(AngleUnit rotationUnit) {
       this.rotationUnit = rotationUnit;
       return this;
     }
@@ -178,18 +169,23 @@ public Builder<P> rotationUnit(AngleUnit rotationUnit) {
      * @param networkTable Table to publish to
      * @return {@code this} for chaining
      */
-    public Builder<P> publishTo(NetworkTable networkTable) {
+    public Builder publishTo(NetworkTable networkTable) {
       this.networkTable = Objects.requireNonNull(networkTable, "networkTable should not be null");
       return this;
     }
 
-    /** Builds a {@code PositionalMotor} using the current values of this builder. */
-    public PositionalMotor<P> build() {
-      return new PositionalMotor<>(this);
+    /**
+     * Builds a {@code PositionalMotor} using the current values of this builder.
+     *
+     * @param initialPosition the initial position of the controller
+     */
+    public <P extends Enum<P> & Supplier<Angle>> PositionalMotor<P> build(P initialPosition) {
+      Objects.requireNonNull(initialPosition, "initialPosition should not be null");
+      return new PositionalMotor<>(this, initialPosition);
     }
   }
 
-  private PositionalMotor(Builder<P> builder) {
+  private PositionalMotor(Builder builder, P initialPosition) {
     controller = builder.controller;
     acceptableError = builder.acceptableError;
     motor = builder.motor;
@@ -205,10 +201,9 @@ private PositionalMotor(Builder<P> builder) {
     }
 
     controller.setTolerance(builder.acceptableError);
-    controller.setSetpoint(builder.initialPosition);
-    encoder.setPosition(rotationUnit.of(builder.initialPosition));
-
-    // TODO: Should we update the position of the controller using controller.calculate()?
+    double initialPositionAsDouble = initialPosition.get().in(builder.rotationUnit);
+    controller.setSetpoint(initialPositionAsDouble);
+    encoder.setPosition(rotationUnit.of(initialPositionAsDouble));
 
     builder.periodicRegistry.addPeriodic(robotState -> this.periodic());
   }