Reworked test for checking drive, not done turning

SFUnity · Sep 2, 2023 · 1f136ac · 1f136ac
1 parent a97a3e8
commit 1f136ac
Showing 1 changed file with 12 additions and 13 deletions.
diff --git a/src/test/java/SwerveModuleTest.java b/src/test/java/SwerveModuleTest.java
@@ -77,22 +77,21 @@ public void testGetState() {
         System.out.println(state.toString());
     }
 
+    /**
+     * @param speed in m/s
+     * @param angle in degrees
+     */
     void setStateTemplate(double speed, double angle) {
         // Arrange
-        double turningPosition = 0.5;
-        when(subsystem.getTurningPosition()).thenReturn(turningPosition);
-        SwerveModuleState unoptimizedState = new SwerveModuleState(speed, new Rotation2d(angle * Math.PI / 180));
-        SwerveModuleState expectedState = new SwerveModuleState(speed, new Rotation2d(angle * Math.PI / 180));
-        expectedState = SwerveModuleState.optimize(expectedState, subsystem.getState().angle);
+        double fakeTurningPosition = 0.5;
+        when(subsystem.getTurningPosition()).thenReturn(fakeTurningPosition);
+        double angleInRadians = angle * Math.PI / 180;
+        SwerveModuleState expectedState = new SwerveModuleState(speed, new Rotation2d(angleInRadians));
         // Act
-        subsystem.setState(unoptimizedState);
-        // Assert
-        double expectedDriveValue = expectedState.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond;
-        if (Math.abs(expectedDriveValue) < 0.001) {
-            expectedDriveValue = Math.abs(expectedDriveValue);
-        }
-        verify(mockDriveMotor).set(expectedDriveValue);
-        verify(mockTurningMotor).set(turningPidController.calculate(subsystem.getTurningPosition(), expectedState.angle.getRadians()));
+        subsystem.setState(expectedState);
+        // Assert                   Sets the drive speed to be proportional to the max speed
+        verify(mockDriveMotor).set(expectedState.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
+        // verify(mockTurningMotor).set();
     }
 
     @Test