Introduce FakeMotor

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

@@ -1,33 +1,31 @@
 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 static org.junit.jupiter.api.Assumptions.*;
 
 import com.team2813.lib2813.control.ControlMode;
-import com.team2813.lib2813.control.PIDMotor;
-import com.team2813.lib2813.testing.FakePIDMotor;
+import com.team2813.lib2813.control.Motor;
+import com.team2813.lib2813.testing.FakeMotor;
 import com.team2813.lib2813.testing.junit.jupiter.CommandTester;
 import com.team2813.lib2813.testing.junit.jupiter.WPILibExtension;
 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) {
+    protected ConcreteParameterizedIntakeSubsystem(Motor intakeMotor, Params params) {
       super(intakeMotor, params);
     }
   }
 
-  final FakePIDMotor fakeMotor = mock(FakePIDMotor.class, Answers.CALLS_REAL_METHODS);
+  private final FakeMotor fakeMotor = new FakeMotor();
   private final ParameterizedIntakeSubsystem.Params params;
 
   public ParameterizedIntakeSubsystemTest(ControlMode controlMode) {
@@ -42,16 +40,15 @@ public ParameterizedIntakeSubsystemTest(ControlMode controlMode) {
   @Test
   public void initialState() {
     try (var ignored = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
-      assertMotorIsStopped();
-      verifyNoInteractions(fakeMotor);
+      fakeMotor.assertIsStopped();
     }
   }
 
   @Test
   public void intakeItem(CommandTester commandTester) {
     try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
       Command command = intake.intakeItemCommand();
-      assertMotorIsStopped();
+      fakeMotor.assertIsStopped();
 
       commandTester.runUntilComplete(command);
 
@@ -61,6 +58,7 @@ public void intakeItem(CommandTester commandTester) {
 
   @Test
   public void stopAfterIntakingItem(CommandTester commandTester) {
+    assumeTrue(!ControlMode.MOTION_MAGIC.equals(params.controlMode()));
     try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
       Command command = intake.intakeItemCommand();
       commandTester.runUntilComplete(command);
@@ -69,7 +67,7 @@ public void stopAfterIntakingItem(CommandTester commandTester) {
 
       commandTester.runUntilComplete(command);
 
-      assertMotorIsStopped();
+      fakeMotor.assertIsStopped();
     }
   }
 
@@ -88,6 +86,7 @@ public void outtakeItem(CommandTester commandTester) {
 
   @Test
   public void stopAfterOuttakingItem(CommandTester commandTester) {
+    assumeTrue(!ControlMode.MOTION_MAGIC.equals(params.controlMode()));
     try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
       intake.intakeGamePiece();
       Command command = intake.outtakeItemCommand();
@@ -97,14 +96,10 @@ public void stopAfterOuttakingItem(CommandTester commandTester) {
 
       commandTester.runUntilComplete(command);
 
-      assertMotorIsStopped();
+      fakeMotor.assertIsStopped();
     }
   }
 
-  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/src/main/java/com/team2813/lib2813/testing/FakeMotor.java

@@ -0,0 +1,106 @@
+package com.team2813.lib2813.testing;
+
+import static com.google.common.truth.Truth.assertThat;
+
+import com.team2813.lib2813.control.ControlMode;
+import com.team2813.lib2813.control.Motor;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Resistance;
+import edu.wpi.first.units.measure.Voltage;
+import java.util.Objects;
+
+/**
+ * A fake implementation of {@link Motor}; used for testing.
+ *
+ * <p>This class simulates motor behavior by storing the most recent control mode and demand value.
+ * It also includes methods that make it easier to verify the current state of the motor.
+ */
+public class FakeMotor implements Motor {
+  private boolean isStopped = true;
+  private double feedForward = 0.0f;
+  public double demand = 0.0f;
+
+  public Resistance resistance = Resistance.ofBaseUnits(0.025f, Units.Ohms);
+  private ControlMode controlMode = ControlMode.VOLTAGE;
+
+  /**
+   * Gets the current voltage being applied to the motor.
+   *
+   * @return applied voltage.
+   * @throws IllegalStateException if the motor's control mode is not {@link ControlMode#VOLTAGE}.
+   */
+  public Voltage getMotorVoltage() {
+    if (!ControlMode.VOLTAGE.equals(controlMode)) {
+      throw new IllegalStateException("Cannot get voltage when controlMode is " + controlMode);
+    }
+    return Voltage.ofBaseUnits(demand, Units.Volts);
+  }
+
+  /**
+   * Gets the feedforward that was applied.
+   *
+   * @return feedforward, in fractional units between -1 and +1.
+   * @throws IllegalStateException if the current control mode does not support feedforward.
+   */
+  public double getFeedForward() {
+    return switch (controlMode) {
+      case VELOCITY, MOTION_MAGIC -> feedForward;
+      default ->
+          throw new IllegalStateException(
+              "Cannot get feedforward when controlMode is " + controlMode);
+    };
+  }
+
+  /**
+   * Assert that the motor is stopped.
+   *
+   * @throws AssertionError if the motor is running.
+   */
+  public final void assertIsStopped() {
+    assertThat(isStopped).isTrue();
+  }
+
+  @Override
+  public final void set(ControlMode mode, double demand) {
+    set(mode, demand, 0);
+  }
+
+  /**
+   * {@inheritDoc}
+   *
+   * @throws AssertionError if the feedForward is not in the range [-1, 1].
+   */
+  @Override
+  public void set(ControlMode mode, double demand, double feedForward) {
+    Objects.requireNonNull(mode, "mode should not be null");
+    if (feedForward >= 0) {
+      assertThat(feedForward).isAtMost(1.01);
+    } else {
+      assertThat(feedForward).isAtLeast(-1.01);
+    }
+
+    if (controlMode != ControlMode.MOTION_MAGIC) {
+      isStopped = (Math.abs(demand) <= 0.0001 && Math.abs(feedForward) <= 0.0001);
+    }
+    this.controlMode = mode;
+    this.demand = demand;
+    this.feedForward = feedForward;
+  }
+
+  /**
+   * {@inheritDoc}
+   *
+   * @throws IllegalStateException if the current control mode does not support feedforward.
+   */
+  @Override
+  public final Current getAppliedCurrent() {
+    try {
+      return getMotorVoltage().div(resistance);
+    } catch (IllegalStateException e) {
+      throw new IllegalStateException("Cannot calculate voltage", e);
+    } catch (RuntimeException e) {
+      throw new RuntimeException("Cannot calculate voltage", e);
+    }
+  }
+}

testing/src/main/java/com/team2813/lib2813/testing/FakePIDMotor.java

@@ -1,35 +1,19 @@
 package com.team2813.lib2813.testing;
 
-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;
+/**
+ * A fake implementation of {@link PIDMotor}; used for testing.
+ *
+ * <p>This class simulates motor behavior by storing the most recent control mode and demand value.
+ * It also includes methods that make it easier to verify the current state of the motor.
+ */
+public abstract class FakePIDMotor extends FakeMotor implements PIDMotor {
 
+  @Deprecated
   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);
+    return getMotorVoltage().in(Units.Volts);
   }
 
   @Override