diff --git a/src/main/java/team/gif/robot/Constants.java b/src/main/java/team/gif/robot/Constants.java index b32bfba..c3f27c4 100644 --- a/src/main/java/team/gif/robot/Constants.java +++ b/src/main/java/team/gif/robot/Constants.java @@ -278,7 +278,7 @@ public static final class Elevator{ public static final double MAX_ACCELERATION = 130; public static final double REV_MAX_ACCELERATION = 80; - public static final double PID_TOLERANCE = 0.1; + public static final double PID_TOLERANCE = 0.3; public static final double MIN_PERCENT_MANUAL = -0.15; public static final double MAX_PERCENT_MANUAL = 0.15; diff --git a/src/main/java/team/gif/robot/commands/shooter/AutoDriveAndShoot.java b/src/main/java/team/gif/robot/commands/shooter/AutoDriveAndShoot.java index d9117f2..b40a913 100644 --- a/src/main/java/team/gif/robot/commands/shooter/AutoDriveAndShoot.java +++ b/src/main/java/team/gif/robot/commands/shooter/AutoDriveAndShoot.java @@ -52,7 +52,7 @@ public void execute() { // Return true when the command should end, false if it should continue. Runs every ~20ms. @Override public boolean isFinished() { - return hasTarget; + return Robot.elevator.isReadyToShoot() && hasTarget; } // Called when the command ends or is interrupted. @@ -62,7 +62,7 @@ public void end(boolean interrupted) { Robot.swerveDrive.setDrivePace(drivePace.COAST_FR); // only shoot if the robot found the target during the command - if (hasTarget) { + if (Robot.elevator.isReadyToShoot() && hasTarget) { // run the shooter using the standard shoot command and return the elevator new SequentialCommandGroup( new Shoot(), diff --git a/src/main/java/team/gif/robot/commands/shooter/AutonStrafeToTarget.java b/src/main/java/team/gif/robot/commands/shooter/AutonStrafeToTarget.java index e6bd39b..423bc31 100644 --- a/src/main/java/team/gif/robot/commands/shooter/AutonStrafeToTarget.java +++ b/src/main/java/team/gif/robot/commands/shooter/AutonStrafeToTarget.java @@ -28,7 +28,7 @@ public void execute() { // This makes sure the sensors don't trip if the robot lined up // correctly and is still raising the elevator // Since this only runs in auto, safe to just chcek height of level 4 - if (Robot.elevator.getPosition() < Constants.Elevator.LEVEL_4_POSITION - 0.3 ) { + if (Robot.elevator.getPosition() < Constants.Elevator.LEVEL_4_POSITION - Constants.Elevator.PID_TOLERANCE) { return; } diff --git a/src/main/java/team/gif/robot/subsystems/Elevator.java b/src/main/java/team/gif/robot/subsystems/Elevator.java index 3e08e1f..31f3ae4 100644 --- a/src/main/java/team/gif/robot/subsystems/Elevator.java +++ b/src/main/java/team/gif/robot/subsystems/Elevator.java @@ -204,6 +204,20 @@ public boolean isMotionMagicFinished() { return Math.abs(PIDError()) < Constants.Elevator.PID_TOLERANCE; } + /** + * Determines if the elevator is up and at its target position. Will return false if + * the elevator is all the way down at the collector position + * + * @return true if the elevator is up ready to shoot, false if not + */ + public boolean isReadyToShoot() { + if(getTargetPosition() == Constants.Elevator.COLLECTOR_POSITION) { + return false; + } + + return Math.abs(getTargetPosition() - getPosition()) < Constants.Elevator.PID_TOLERANCE; + } + /** * gets the motor output voltage *