Rewrote setState bc of issue w/ stopMotors

SFUnity · Sep 1, 2023 · 5b22552 · 5b22552
1 parent 607d8b0
commit 5b22552
Showing 1 changed file with 10 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -124,21 +124,22 @@ public SwerveModuleState getState() {
      */
     public void setState(SwerveModuleState state) {
         if (Math.abs(state.speedMetersPerSecond) < 0.001) {
-            stopMotors();
-            return;
+            m_driveMotor.set(0);
+        } else {
+            m_driveMotor.set(state.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
         }
         state = SwerveModuleState.optimize(state, getState().angle);
-        m_driveMotor.set(state.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
-        m_turningMotor.set(turningPidController.calculate(getTurningPosition(), state.angle.getRadians()));
+        double desiredTurnValue = turningPidController.calculate(getTurningPosition(), state.angle.getRadians());
+        if (Math.abs(desiredTurnValue) < 0.001) {
+            m_turningMotor.set(0);
+        } else {
+            m_turningMotor.set(desiredTurnValue);
+        }
+
         SmartDashboard.putString("Swerve[" + m_absoluteEncoder.getChannel() + "] state", state.toString());
         System.out.println("Swerve[" + m_absoluteEncoder.getChannel() + "] state " + state.toString());
     }
 
-    public void stopMotors() {
-        m_driveMotor.set(0);
-        m_turningMotor.set(0);
-    }
-
     @Override
     public void close() throws Exception {
         m_driveMotor.close();