diff --git a/src/main/java/frc/robot/subsystems/modules/RealSwerveModule.java b/src/main/java/frc/robot/subsystems/modules/RealSwerveModule.java index f6316204..34882f80 100644 --- 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 index b3f03df9..b0cbb16d 100644 --- 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 argumentCaptor = ArgumentCaptor.forClass(Double.class); verify(mockTurningMotor).set(argumentCaptor.capture());