diff --git a/src/main/java/com/team1816/lib/auto/PathfindManager.java b/src/main/java/com/team1816/lib/auto/PathfindManager.java index f591549c..516e1173 100644 --- a/src/main/java/com/team1816/lib/auto/PathfindManager.java +++ b/src/main/java/com/team1816/lib/auto/PathfindManager.java @@ -54,6 +54,11 @@ public void addPath(PathfindToPoseCommand cmd) { } public void startPathfinding() { + if (currentPathCommand != null) { + GreenLogger.log("Canceled current path command"); + currentPathCommand.cancel(); + } + currentPathCommand = commandChooser.getSelected(); if (currentPathCommand == null) { diff --git a/src/main/java/com/team1816/lib/commands/PathfindToPoseCommand.java b/src/main/java/com/team1816/lib/commands/PathfindToPoseCommand.java index 3fc716fd..dc3d2c56 100644 --- a/src/main/java/com/team1816/lib/commands/PathfindToPoseCommand.java +++ b/src/main/java/com/team1816/lib/commands/PathfindToPoseCommand.java @@ -24,6 +24,8 @@ public PathfindToPoseCommand(String pathName, this.constraints = constraints; } + private static final double CONTROLLER_DEADBAND = 0.01; + private Command internalCommand; private final Pose2d targetPose; @@ -91,8 +93,9 @@ private boolean shouldFlip() { } private boolean controllerInputDetected() { - return controller.getLeftY() != 0 - || controller.getLeftX() != 0 - || controller.getRightX() != 0; + return Math.abs(controller.getLeftX()) >= CONTROLLER_DEADBAND + || Math.abs(controller.getLeftY()) >= CONTROLLER_DEADBAND + || Math.abs(controller.getRightX()) >= CONTROLLER_DEADBAND + || Math.abs(controller.getRightY()) >= CONTROLLER_DEADBAND; } }