Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
refactor(swerve): Change how module speed deadband is calculated.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 27, 2024
1 parent 4642109 commit 5630e26
Showing 1 changed file with 3 additions and 6 deletions.
9 changes: 3 additions & 6 deletions src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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;

Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit 5630e26

Please sign in to comment.