Added unit tests for SwerveModule.setToMoveAt()

src/test/java/org/bitbuckets/drive/SwerveModuleTest.java

@@ -1,28 +1,102 @@
 package org.bitbuckets.drive;
 
-import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
-import org.mockito.Mockito;
+import org.mockito.AdditionalMatchers;
+import xyz.auriium.mattlib2.hardware.ILinearMotor;
 import xyz.auriium.mattlib2.hardware.IRotationEncoder;
-import xyz.auriium.mattlib2.hardware.IRotationalMotor;
+import xyz.auriium.mattlib2.hardware.IRotationalController;
 
+import static org.bitbuckets.RobotContainer.SWERVE;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.mockito.Mockito.*;
 
 public class SwerveModuleTest {
 
-    @Test
-    public void TestLogicPeriodic() {
+    ILinearMotor driveMotor;
+    IRotationalController steerController;
+    IRotationEncoder absoluteEncoder;
+    SimpleMotorFeedforward ff;
+
+    @BeforeEach
+    public void beforeEach() {
+        driveMotor = mock(ILinearMotor.class);
+        steerController = mock(IRotationalController.class);
+        absoluteEncoder = mock(IRotationEncoder.class);
+        ff = new SimpleMotorFeedforward(SWERVE.ff_ks(), SWERVE.ff_kv());
 
-        IRotationalMotor steerController = Mockito.mock(IRotationalMotor.class);
-        Mockito.when(steerController.angularVelocity_mechanismRotationsPerSecond()).thenReturn(20d);
+    }
+
+    @Test
+    public void testLogicPeriodic() {
+        when(steerController.angularVelocity_mechanismRotationsPerSecond()).thenReturn(20d);
+        when(absoluteEncoder.angularPosition_normalizedMechanismRotations()).thenReturn(10d);
 
-        IRotationEncoder absoluteEncoder = Mockito.mock(IRotationEncoder.class);
-        Mockito.when(absoluteEncoder.angularPosition_normalizedMechanismRotations()).thenReturn(10d);
         double absoluteAngularPosition_infiniteMechanismRotations = absoluteEncoder.angularPosition_normalizedMechanismRotations();
-        System.out.print(steerController.angularPosition_mechanismRotations());
         steerController.forceRotationalOffset(absoluteAngularPosition_infiniteMechanismRotations);
 
         assertEquals(0d, steerController.angularPosition_mechanismRotations(), 0);
     }
 
+    @Test
+    public void testSetToMoveAtStraight() {
+        SwerveModule module = new SwerveModule(driveMotor, steerController, absoluteEncoder, ff);
+
+        module.setToMoveAt(new SwerveModuleState(20, Rotation2d.fromDegrees(0)));
+        verify(driveMotor).setToVoltage(12);
+        verify(steerController).controlToNormalizedReference(0);
+    }
+
+    @Test
+    public void testSetToMoveAtTurn1() {
+        SwerveModule module = new SwerveModule(driveMotor, steerController, absoluteEncoder, ff);
+
+        // move a quarter
+        module.setToMoveAt(new SwerveModuleState(20, Rotation2d.fromDegrees(90)));
+        verify(steerController).controlToNormalizedReference(.25);
+    }
+
+    @Test
+    public void testSetToMoveAtTurn2() {
+        SwerveModule module = new SwerveModule(driveMotor, steerController, absoluteEncoder, ff);
+
+        // move a quarter
+        module.setToMoveAt(new SwerveModuleState(20, Rotation2d.fromDegrees(-90)));
+        verify(steerController).controlToNormalizedReference(.75);
+    }
+
+    @Test
+    public void testSetToMoveAtTurn3() {
+        SwerveModule module = new SwerveModule(driveMotor, steerController, absoluteEncoder, ff);
+
+        // abs encoder registers 1/2 turn clockwise
+        when(steerController.angularPosition_normalizedMechanismRotations()).thenReturn(.5);
+
+        // rotate a bit counter-clockwise
+        // the "asked for" rotation would be -.7, but we flip the drive motor in reverse and only
+        // actually rotate .3
+        module.setToMoveAt(new SwerveModuleState(20, Rotation2d.fromDegrees(-72)));
+
+        // the drive motor should move backwards, and the mechanism should be set to .4 (-.1 + .5)
+        verify(driveMotor).setToVoltage(-12);
+        verify(steerController).controlToNormalizedReference(AdditionalMatchers.eq(.3, .01));
+    }
+
+    @Test
+    public void testSetToMoveAtTurn4() {
+        SwerveModule module = new SwerveModule(driveMotor, steerController, absoluteEncoder, ff);
+
+        // abs encoder registers 1/4 turn clockwise
+        when(steerController.angularPosition_normalizedMechanismRotations()).thenReturn(.25);
+
+        // rotate a bit counter-clockwise
+        module.setToMoveAt(new SwerveModuleState(20, Rotation2d.fromDegrees(-36)));
+
+        // the drive motor should move backwards, and the mechanism should be set to .4 (-.1 + .5)
+        verify(driveMotor).setToVoltage(-12);
+        verify(steerController).controlToNormalizedReference(.4);
+    }
 }