Fixed opposites error caused by .optimize()

SFUnity · Sep 18, 2023 · 894323a · 894323a
1 parent e788747
commit 894323a
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/subsystems/modules/RealSwerveModule.java b/src/main/java/frc/robot/subsystems/modules/RealSwerveModule.java
@@ -85,10 +85,10 @@ public SwerveModulePosition getPosition() {
 
     @Override
     public void setDesiredState(SwerveModuleState state) {
-        m_driveMotor.set(state.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
         state = SwerveModuleState.optimize(state, getState().angle);
         double desiredTurnSpeed = turningPidController.calculate(getTurningPosition(), state.angle.getRadians());
         m_turningMotor.set(desiredTurnSpeed);
+        m_driveMotor.set(state.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
 
         desiredState = state;
         SmartDashboard.putString("Swerve[" + m_absoluteEncoder.getChannel() + "] state", state.toString());

diff --git a/src/test/java/RealSwerveModuleTest.java b/src/test/java/RealSwerveModuleTest.java
@@ -77,8 +77,9 @@ void setDesiredStateTemplate(double speed, double angle) {
         SwerveModuleState expectedState = new SwerveModuleState(speed, new Rotation2d(angleInRadians));
         // Act
         subsystem.setDesiredState(expectedState);
-        // Assert                   Sets the drive speed to be proportional to the max speed
-        verify(mockDriveMotor).set(expectedState.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
+        // Assert
+        expectedState = SwerveModuleState.optimize(expectedState, subsystem.getState().angle);  
+        verify(mockDriveMotor).set(expectedState.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond); // Sets the drive speed to be proportional to the max speed
         // Verifies that the speed of the turning motor is appropriate given optimization
         ArgumentCaptor<Double> argumentCaptor = ArgumentCaptor.forClass(Double.class);
         verify(mockTurningMotor).set(argumentCaptor.capture());