From 5b2255279ca0e422f7b500b1e31bb8bf0867975a Mon Sep 17 00:00:00 2001 From: MrTinker64 Date: Thu, 31 Aug 2023 18:55:21 -0700 Subject: [PATCH] Rewrote setState bc of issue w/ stopMotors --- .../frc/robot/subsystems/SwerveModule.java | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index dc5a6faa..31eefe46 100644 --- 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();