diff --git a/src/main/java/frc/robot/autos/P123.java b/src/main/java/frc/robot/autos/P123.java index 7c1abb71..2d7dcd75 100644 --- a/src/main/java/frc/robot/autos/P123.java +++ b/src/main/java/frc/robot/autos/P123.java @@ -65,6 +65,7 @@ public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Inde Command followPath6 = AutoBuilder.followPath(path6); BooleanSupplier abort = indexer.noteInIndexer.negate().and(intake.noteInIntake.negate()); + BooleanSupplier goToCenter = () -> RobotContainer.goToCenter.getEntry().getBoolean(false); Command resetPosition = Commands.runOnce(() -> { Pose2d initialState = @@ -90,15 +91,15 @@ public P123(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Inde .andThen(CommandFactory.Auto.runIndexer(indexer)) .andThen(elevatorWrist.homePosition().withTimeout(.5)); Command part5 = followPath5.andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake, indexer), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), CommandFactory.Auto.runIndexer(indexer)), abort)); Command part6 = followPath6.andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake, indexer), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), CommandFactory.Auto.runIndexer(indexer)), abort)); - Command midline = Commands.either(Commands.sequence(part5, part6), Commands.none(), - () -> RobotContainer.goToCenter.getEntry().getBoolean(false)); + Command midline = + Commands.either(Commands.sequence(part5, part6), Commands.none(), goToCenter); Command followPaths = Commands.sequence(part1, part2, part3, part4, midline); diff --git a/src/main/java/frc/robot/autos/P321.java b/src/main/java/frc/robot/autos/P321.java index 6af30ecf..cbf3b0e6 100644 --- a/src/main/java/frc/robot/autos/P321.java +++ b/src/main/java/frc/robot/autos/P321.java @@ -62,7 +62,7 @@ public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Inde Command followPath6 = AutoBuilder.followPath(path6); BooleanSupplier abort = indexer.noteInIndexer.negate().and(intake.noteInIntake.negate()); - + BooleanSupplier goToCenter = () -> RobotContainer.goToCenter.getEntry().getBoolean(false); Command resetPosition = Commands.runOnce(() -> { Pose2d initialState = @@ -88,15 +88,15 @@ public P321(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Inde .andThen(CommandFactory.Auto.runIndexer(indexer)) .andThen(elevatorWrist.homePosition().withTimeout(.5)); Command part5 = followPath5.andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake, indexer), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), CommandFactory.Auto.runIndexer(indexer)), abort)); Command part6 = followPath6.andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake, indexer), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), CommandFactory.Auto.runIndexer(indexer)), abort)); - Command midline = Commands.either(Commands.sequence(part5, part6), Commands.none(), - () -> RobotContainer.goToCenter.getEntry().getBoolean(false)); + Command midline = + Commands.either(Commands.sequence(part5, part6), Commands.none(), goToCenter); Command followPaths = Commands.sequence(part1, part2, part3, part4, midline); diff --git a/src/main/java/frc/robot/autos/P8765.java b/src/main/java/frc/robot/autos/P8765.java index 6e07e64c..a9b00727 100644 --- a/src/main/java/frc/robot/autos/P8765.java +++ b/src/main/java/frc/robot/autos/P8765.java @@ -68,8 +68,7 @@ public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Ind Command followPath3_dump = AutoBuilder.followPath(path3_dump); Command followPath4_dump = AutoBuilder.followPath(path4_dump); - BooleanSupplier abort = - () -> !indexer.noteInIndexer.getAsBoolean() && !intake.noteInIntake.getAsBoolean(); + BooleanSupplier abort = indexer.noteInIndexer.negate().and(intake.noteInIntake.negate()); BooleanSupplier dumpOrNot = () -> RobotContainer.dumpNotes.getEntry().getBoolean(false); double elevatorHeight = 28.0; Command part0 = followPath0 @@ -79,23 +78,19 @@ public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Ind .andThen(Commands.waitSeconds(.1)).andThen(CommandFactory.Auto.runIndexer(indexer)); // .andThen(Commands.either(elevatorWrist.homePosition().withTimeout(.5), Commands.none(), // dumpOrNot)); - Command part1 = followPath1.deadlineWith(CommandFactory.intakeNote(intake, indexer), - elevatorWrist.homePosition().withTimeout(1.0)) - .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake, indexer).alongWith(elevatorWrist + Command part1 = + followPath1.deadlineWith(elevatorWrist.homePosition().withTimeout(1.0)) + .andThen(Commands.either(Commands.none(), Commands.sequence(elevatorWrist .followPosition(() -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, () -> elevatorWrist.getAngleFromDistance(swerveDrive.getPose())) - .withTimeout(1.5)), CommandFactory.Auto.runIndexer(indexer)), - abort)); - Command part2 = followPath2.deadlineWith(CommandFactory.intakeNote(intake, indexer), - elevatorWrist.homePosition().withTimeout(1.0)) - .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake, indexer).alongWith(elevatorWrist + .withTimeout(1.5), CommandFactory.Auto.runIndexer(indexer)), abort)); + Command part2 = + followPath2.deadlineWith(elevatorWrist.homePosition().withTimeout(1.0)) + .andThen(Commands.either(Commands.none(), Commands.sequence(elevatorWrist .followPosition(() -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, () -> elevatorWrist.getAngleFromDistance(swerveDrive.getPose())) - .withTimeout(1.5)), CommandFactory.Auto.runIndexer(indexer)), - abort)); - Command part3 = followPath3.alongWith(CommandFactory.intakeNote(intake, indexer)) + .withTimeout(1.5), CommandFactory.Auto.runIndexer(indexer)), abort)); + Command part3 = followPath3 .andThen( elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, Rotation2d.fromDegrees(32.5)).withTimeout(1.5)) @@ -103,28 +98,25 @@ public P8765(Swerve swerveDrive, ElevatorWrist elevatorWrist, Intake intake, Ind .andThen(elevatorWrist.homePosition().withTimeout(.5)); - Command part1_dump = - followPath1_dump.deadlineWith(CommandFactory.intakeNote(intake, indexer)) - .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake, indexer), - CommandFactory.Auto.runIndexer(indexer)), - abort)) - .andThen(elevatorWrist.homePosition().withTimeout(.1)); - Command part2_dump = - followPath2_dump.deadlineWith(CommandFactory.intakeNote(intake, indexer)) - .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake, indexer), - CommandFactory.Auto.runIndexer(indexer)), - abort)) - .andThen(elevatorWrist.homePosition().withTimeout(.1)); - Command part3_dump = - followPath3_dump.deadlineWith(CommandFactory.intakeNote(intake, indexer)) - .andThen(Commands.either(Commands.none(), - Commands.sequence(CommandFactory.intakeNote(intake, indexer), - CommandFactory.Auto.runIndexer(indexer)), - abort)) - .andThen(elevatorWrist.homePosition().withTimeout(.1)); - Command part4_dump = followPath4_dump.alongWith(CommandFactory.intakeNote(intake, indexer)); + Command part1_dump = followPath1_dump + .andThen(Commands.either(Commands.none(), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer)), + abort)) + .andThen(elevatorWrist.homePosition().withTimeout(.1)); + Command part2_dump = followPath2_dump + .andThen(Commands.either(Commands.none(), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer)), + abort)) + .andThen(elevatorWrist.homePosition().withTimeout(.1)); + Command part3_dump = followPath3_dump + .andThen(Commands.either(Commands.none(), + Commands.sequence(Commands.waitUntil(indexer.noteInIndexer), + CommandFactory.Auto.runIndexer(indexer)), + abort)) + .andThen(elevatorWrist.homePosition().withTimeout(.1)); + Command part4_dump = followPath4_dump; Command wait = Commands.waitSeconds(.01); Command resetPosition = Commands.runOnce(() -> {