maybe finsih autos??

Frc5572 · Sep 29, 2024 · 6177caa · 6177caa
1 parent b595aa6
commit 6177caa
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 46 deletions.
diff --git 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
@@ -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
@@ -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,52 +78,45 @@ 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))
             .andThen(CommandFactory.Auto.runIndexer(indexer))
             .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(() -> {