Param intake

lib/build.gradle

@@ -29,10 +29,14 @@ dependencies {
 
     testImplementation(platform('org.junit:junit-bom:5.13.1'))
     testImplementation('org.junit.jupiter:junit-jupiter')
+    testImplementation 'org.mockito:mockito-core:5.14.2'
     testRuntimeOnly('org.junit.platform:junit-platform-launcher')
     testRuntimeOnly('org.junit.vintage:junit-vintage-engine')
     testImplementation 'junit:junit:4.13.2'
-    testImplementation 'com.google.truth:truth:1.4.4'
+    implementation 'com.google.truth:truth:1.4.4'//needed for FakePIDMotor
+    testImplementation project(':testing')
+    compileOnly 'com.google.auto.value:auto-value-annotations:1.11.0'
+    annotationProcessor 'com.google.auto.value:auto-value:1.11.0'
 }
 
 // the magic line that makes tests work :)

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

@@ -0,0 +1,106 @@
+package com.team2813.lib2813.subsystems;
+
+import com.google.auto.value.AutoBuilder;
+import com.team2813.lib2813.control.ControlMode;
+import com.team2813.lib2813.control.PIDMotor;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class ParameterizedIntakeSubsystem extends SubsystemBase implements AutoCloseable {
+  private final PIDMotor intakeMotor;
+  private final Params params;
+
+  public record Params(ControlMode controlMode, double intakeDemand, double outtakeDemand) {
+
+    public static Params.Builder builder() {
+      return new AutoBuilder_ParameterizedIntakeSubsystem_Params_Builder()
+          .setControlMode(ControlMode.VOLTAGE);
+    }
+
+    @AutoBuilder
+    public interface Builder {
+      Builder setControlMode(ControlMode controlMode);
+
+      Builder setIntakeDemand(double demand);
+
+      Builder setOuttakeDemand(double demand);
+
+      Params build();
+    }
+
+    public Params {
+      if (controlMode == null) {
+        throw new IllegalArgumentException("controlMode cannot be null");
+      }
+      if (isEssentiallyZero(intakeDemand)) {
+        throw new IllegalArgumentException("intakeDemand cannot be zero");
+      }
+      if (isEssentiallyZero(outtakeDemand)) {
+        throw new IllegalArgumentException("outtakeDemand cannot be zero");
+      }
+      if (Math.signum(intakeDemand) == Math.signum(outtakeDemand)) {
+        throw new IllegalArgumentException(
+            "intakeDemand should be the opposite sign of outtakeDemand");
+      }
+    }
+  }
+
+  protected ParameterizedIntakeSubsystem(PIDMotor intakeMotor, Params params) {
+    this.intakeMotor = intakeMotor;
+    this.params = params;
+  }
+
+  public final Command intakeItemCommand() {
+    return new InstantCommand(this::intakeGamePiece, this);
+  }
+
+  public final Command outtakeItemCommand() {
+    return new InstantCommand(this::outtakeGamePiece, this);
+  }
+
+  public final Command stopMotorCommand() {
+    return new InstantCommand(this::stopMotor, this);
+  }
+
+  /** Makes intake wheels spin in the intake direction. */
+  protected final void intakeGamePiece() {
+    // FIXME: Maybe add a check that the wheels are not stalled.
+    setMotorDemand(params.intakeDemand);
+  }
+
+  /** Makes intake wheels spin in the outtake direction. */
+  protected final void outtakeGamePiece() {
+    setMotorDemand(params.outtakeDemand);
+  }
+
+  /**
+   * Runs the motor with the provided demand value.
+   *
+   * @param demand Demand of the motor. Meaning depends on the {@code ControlMode}.
+   */
+  protected final void setMotorDemand(double demand) {
+    intakeMotor.set(params.controlMode, demand);
+  }
+
+  /**
+   * Returns a command that runs the motor with the provided demand value.
+   *
+   * @param demand Demand of the motor. Meaning depends on the {@code ControlMode}.
+   */
+  protected final Command setMotorDemandCommand(double demand) {
+    return new InstantCommand(() -> setMotorDemand(demand), this);
+  }
+
+  /** Stops the motor. */
+  public final void stopMotor() {
+    setMotorDemand(0.0);
+  }
+
+  @Override
+  public void close() {}
+
+  private static boolean isEssentiallyZero(double value) {
+    return Math.abs(value) < 0.001;
+  }
+}

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

@@ -0,0 +1,39 @@
+package com.team2813.lib2813.util;
+
+import com.google.common.truth.Truth;
+import com.team2813.lib2813.control.ControlMode;
+import com.team2813.lib2813.control.PIDMotor;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.AngularVelocity;
+
+public abstract class FakePIDMotor implements PIDMotor {
+  public double demand = 0.0f;
+  private ControlMode controlMode;
+
+  public double getVoltage() {
+    Truth.assertThat(controlMode).isEqualTo(ControlMode.VOLTAGE);
+    return demand;
+  }
+
+  @Override
+  public void set(ControlMode mode, double demand) {
+    Truth.assertThat(mode).isNotNull();
+    controlMode = mode;
+    this.demand = demand;
+  }
+
+  @Override
+  public void set(ControlMode mode, double demand, double feedForward) {
+    set(mode, demand);
+  }
+
+  @Override
+  public AngularVelocity getVelocityMeasure() {
+    return Units.RadiansPerSecond.of(demand * 20);
+  }
+
+  @Override
+  public double getVelocity() {
+    throw new AssertionError("Called deprecated method");
+  }
+}

lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java

@@ -0,0 +1,111 @@
+package com.team2813.lib2813.subsystems;
+
+import static com.google.common.truth.Truth.assertThat;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.verifyNoInteractions;
+
+import com.team2813.lib2813.control.ControlMode;
+import com.team2813.lib2813.control.PIDMotor;
+import com.team2813.lib2813.testing.junit.jupiter.CommandTester;
+import com.team2813.lib2813.testing.junit.jupiter.WPILibExtension;
+import com.team2813.lib2813.util.FakePIDMotor;
+import edu.wpi.first.wpilibj2.command.Command;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.extension.ExtendWith;
+import org.junit.jupiter.params.ParameterizedClass;
+import org.junit.jupiter.params.provider.EnumSource;
+import org.mockito.Answers;
+
+@ParameterizedClass
+@EnumSource(ControlMode.class)
+@ExtendWith(WPILibExtension.class)
+public final class ParameterizedIntakeSubsystemTest {
+
+  private static class ConcreteParameterizedIntakeSubsystem extends ParameterizedIntakeSubsystem {
+    protected ConcreteParameterizedIntakeSubsystem(PIDMotor intakeMotor, Params params) {
+      super(intakeMotor, params);
+    }
+  }
+
+  final FakePIDMotor fakeMotor = mock(FakePIDMotor.class, Answers.CALLS_REAL_METHODS);
+  private final ParameterizedIntakeSubsystem.Params params;
+
+  public ParameterizedIntakeSubsystemTest(ControlMode controlMode) {
+    params =
+        ParameterizedIntakeSubsystem.Params.builder()
+            .setControlMode(controlMode)
+            .setIntakeDemand(42)
+            .setOuttakeDemand(-3.1415)
+            .build();
+  }
+
+  @Test
+  public void initialState() {
+    try (var ignored = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
+      assertMotorIsStopped();
+      verifyNoInteractions(fakeMotor);
+    }
+  }
+
+  @Test
+  public void intakeItem(CommandTester commandTester) {
+    try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
+      Command command = intake.intakeItemCommand();
+      assertMotorIsStopped();
+
+      commandTester.runUntilComplete(command);
+
+      assertThat(fakeMotor.demand).isWithin(0.01).of(params.intakeDemand());
+    }
+  }
+
+  @Test
+  public void stopAfterIntakingItem(CommandTester commandTester) {
+    try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
+      Command command = intake.intakeItemCommand();
+      commandTester.runUntilComplete(command);
+      command = intake.stopMotorCommand();
+      assertMotorIsRunning();
+
+      commandTester.runUntilComplete(command);
+
+      assertMotorIsStopped();
+    }
+  }
+
+  @Test
+  public void outtakeItem(CommandTester commandTester) {
+    try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
+      intake.intakeGamePiece();
+      Command command = intake.outtakeItemCommand();
+      assertMotorIsRunning();
+
+      commandTester.runUntilComplete(command);
+
+      assertThat(fakeMotor.demand).isWithin(0.01).of(params.outtakeDemand());
+    }
+  }
+
+  @Test
+  public void stopAfterOuttakingItem(CommandTester commandTester) {
+    try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
+      intake.intakeGamePiece();
+      Command command = intake.outtakeItemCommand();
+      commandTester.runUntilComplete(command);
+      command = intake.stopMotorCommand();
+      assertMotorIsRunning();
+
+      commandTester.runUntilComplete(command);
+
+      assertMotorIsStopped();
+    }
+  }
+
+  private void assertMotorIsStopped() {
+    assertThat(fakeMotor.demand).isWithin(0.01).of(0.0);
+  }
+
+  private void assertMotorIsRunning() {
+    assertThat(fakeMotor.demand).isNotWithin(0.01).of(0.0);
+  }
+}

testing/build.gradle

@@ -20,7 +20,6 @@ dependencies {
     implementation 'com.google.truth:truth:1.4.4'
     implementation(platform('org.junit:junit-bom:5.13.1'))
     implementation('org.junit.jupiter:junit-jupiter')
-
     nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
     nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
     simulationDebug wpi.sim.enableDebug()