run intake when elevator goes up to "send" it into the ampper a littl…

…e bit more
Patribots4738 · Apr 18, 2024 · b085614 · b085614
1 parent d26f974
commit b085614
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -497,7 +497,10 @@ private void configureSoloDiverBindings(PatriBoxController controller) {
             .onFalse(pieceControl.stopIntakeAndIndexer().alongWith(shooterCmds.raisePivot()));
 
         controller.b()
-            .onTrue(pieceControl.noteToTrap3().andThen(elevator.toTopCommand()).andThen(pieceControl.prepPiece()));
+            .onTrue(pieceControl.noteToTrap3()
+                .andThen(
+                    elevator.toTopCommand().deadlineWith(intake.inCommand()).andThen(intake.stopCommand()), 
+                    pieceControl.prepPiece()));
 
         // If this is nice to work with, then we keep it. If not... bye bye!
         new Trigger(() -> elevator.getDesiredPosition() == ElevatorConstants.TRAP_PLACE_POS 

diff --git a/src/main/java/frc/robot/commands/managers/PieceControl.java b/src/main/java/frc/robot/commands/managers/PieceControl.java
@@ -189,7 +189,7 @@ public Command intakeForDoubleAmp() {
     }
 
     public Command doubleAmpElevatorEnd() {
-        return Commands.either(elevatorToTop(), elevatorToBottom(), this::doubleAmpTimerReady);
+        return Commands.either(elevatorToTop().deadlineWith(intake.inCommand()).andThen(intake.stopCommand()), elevatorToBottom(), this::doubleAmpTimerReady);
     }
 
     public Command blepNote() {