From 5630e261ab87afc90c1e9fb4c57e0a42190a3637 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 27 Apr 2024 16:19:21 -0400 Subject: [PATCH] refactor(swerve): Change how module speed deadband is calculated. --- src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java index 4d336e9..5774f14 100644 --- a/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java +++ b/src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java @@ -1,9 +1,9 @@ package frc.robot.swerve; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import frc.lib.controller.PositionControllerIO; import frc.lib.controller.PositionControllerIO.PositionControllerIOValues; import frc.lib.controller.VelocityControllerIO; @@ -25,9 +25,6 @@ public class SwerveModuleIOCustom implements SwerveModuleIO { /** Drive motor values. */ private final VelocityControllerIOValues driveMotorValues = new VelocityControllerIOValues(); - /** Speeds below this speed are zeroed. */ - private final double kSpeedDeadbandMetersPerSecond = 0.025; - /** Module setpoint */ private SwerveModuleState setpoint; @@ -92,11 +89,11 @@ private SwerveModuleState optimize( // Since we are lazy, perform additional optimizations // Deadband the module speed - if (MathUtil.isNear(0.0, setpoint.speedMetersPerSecond, kSpeedDeadbandMetersPerSecond)) { + if (Math.abs(setpoint.speedMetersPerSecond) < Units.inchesToMeters(1)) { setpoint.speedMetersPerSecond = 0.0; } - // Keep previous angle if we aren't moving + // Keep previous angle if the module isn't moving if (setpoint.speedMetersPerSecond == 0.0) { setpoint.angle = state.angle; }