diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 1c23374..5c9b778 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -19,6 +19,8 @@ public class DriveCommand extends CommandBase { static final double Y_DEADBAND = 0.10; + static final double LEFT_STICK_DEADBAND = 0.10; + static final double THETA_DEADBAND = 0.10; static final double X_MAX_ACCELERATION = 0.02; @@ -84,9 +86,9 @@ public void execute() { double nextYSpeed = ySpeed.getAsDouble(); double nextThetaSpeed = thetaSpeed.getAsDouble(); - // Apply individual deadbands. - nextXSpeed = ControlsUtilities.applyDeadband(nextXSpeed, DriveCommand.X_DEADBAND); - nextYSpeed = ControlsUtilities.applyDeadband(nextYSpeed, DriveCommand.Y_DEADBAND); + // Apply individual deadbands. + nextXSpeed = ControlsUtilities.exceedsCircularDeadband(nextXSpeed, nextYSpeed, DriveCommand.LEFT_STICK_DEADBAND) ? 0 : nextXSpeed; + nextYSpeed = ControlsUtilities.exceedsCircularDeadband(nextXSpeed, nextYSpeed, DriveCommand.LEFT_STICK_DEADBAND) ? 0 : nextYSpeed; nextThetaSpeed = ControlsUtilities.applyDeadband(nextThetaSpeed, DriveCommand.THETA_DEADBAND); double absoluteNextXSpeed = Math.abs(nextXSpeed); @@ -99,22 +101,16 @@ public void execute() { absoluteNextYSpeed < DriveCommand.Y_DEADBAND && absoluteNextThetaSpeed < DriveCommand.THETA_DEADBAND ) { - nextXSpeed = 0; nextYSpeed = 0; nextThetaSpeed = 0; - } else { - // Enforce maximum change in acceleration. nextXSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentXSpeed, nextXSpeed, X_MAX_ACCELERATION); nextYSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentYSpeed, nextYSpeed, Y_MAX_ACCELERATION); nextThetaSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentThetaSpeed, nextThetaSpeed, THETA_MAX_ACCELERATION); - } - - this.speedMultiplier = slowMode.getAsBoolean() ? 0.25 : 1; if (turnAround.getAsBoolean() && !wasOneEighty) { diff --git a/src/main/java/frc/robot/util/ControlsUtilities.java b/src/main/java/frc/robot/util/ControlsUtilities.java index 70814c5..d873b1f 100644 --- a/src/main/java/frc/robot/util/ControlsUtilities.java +++ b/src/main/java/frc/robot/util/ControlsUtilities.java @@ -24,6 +24,11 @@ public static double applyDeadband(double input, double deadband) { return Math.abs(input) < deadband ? 0 : input; } + + public static boolean exceedsCircularDeadband (double xInput, double yInput, double deadband) { + + return Math.abs(Math.sqrt(Math.pow(xInput, 2) + Math.pow(yInput, 2))) < deadband; + } /** * Returns the new value so long as its delta from the old value does not