Merge pull request #12 from SciBorgs/simple-motor

Simple motor

src/main/java/org/sciborgs1155/lib/SimpleMotor.java

@@ -0,0 +1,34 @@
+package org.sciborgs1155.lib;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import java.util.function.DoubleConsumer;
+
+public class SimpleMotor {
+  private final DoubleConsumer set;
+  private final Runnable close;
+
+  public SimpleMotor(DoubleConsumer set, Runnable close) {
+    this.set = set;
+    this.close = close;
+  }
+
+  public static SimpleMotor talon(TalonFX talon, TalonFXConfiguration config) {
+    FaultLogger.register(talon);
+    TalonUtils.addMotor(talon);
+    talon.getConfigurator().apply(config);
+    return new SimpleMotor(talon::set, talon::close);
+  }
+
+  public static SimpleMotor none() {
+    return new SimpleMotor(v -> {}, () -> {});
+  }
+
+  public void set(double power) {
+    set.accept(power);
+  }
+
+  public void close() {
+    close.run();
+  }
+}

src/test/java/org/sciborgs1155/lib/SimpleMotorTest.java

@@ -0,0 +1,38 @@
+package org.sciborgs1155.lib;
+
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.sciborgs1155.lib.UnitTestingUtil.*;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj2.command.Commands;
+import org.junit.jupiter.api.Test;
+
+public class SimpleMotorTest {
+  @Test
+  public void test() throws Exception {
+    setupTests();
+    DCMotorSim motor =
+        new DCMotorSim(LinearSystemId.createDCMotorSystem(1, 0.3), DCMotor.getKrakenX60(1));
+
+    assert motor.getAngularAcceleration().in(RadiansPerSecondPerSecond) == 0;
+
+    SimpleMotor sm = new SimpleMotor(motor::setInput, () -> {});
+    run(Commands.run(() -> motor.update(0.02)));
+
+    sm.set(0.5);
+    assertEquals(motor.getInput().get(0, 0), 0.5);
+
+    fastForward();
+    assertEquals(motor.getAngularVelocityRadPerSec(), 0.5, 2e-3);
+
+    sm.set(-0.5);
+
+    fastForward();
+    assertEquals(motor.getAngularVelocityRadPerSec(), -0.5, 2e-3);
+
+    reset();
+  }
+}