update the note position, then stop the motors

Patribots4738 · Jan 24, 2024 · 36e6e60 · 36e6e60
1 parent 3940508
commit 36e6e60
Showing 1 changed file with 13 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/commands/handoff/Handoff.java b/src/main/java/frc/robot/commands/handoff/Handoff.java
@@ -68,7 +68,7 @@ public Command stopAllMotors() {
     }
 
     // TODO: We assume that the note is already in the claw when we want to shoot
-    public Command clawToShooter(){
+    public Command noteToShoot(){
         // this.notePosition = NotePosition.CLAW; ^^^
         Command shoot = emptyCommand;
         this.shooterCalc.prepareFireMovingCommand(() -> true, swerve);
@@ -78,27 +78,28 @@ public Command clawToShooter(){
                 indexer.toShooter()
                     .andThen(intake.inCommand()) // TODO: is this correct? because the top and bottom motors are seperate
                     .andThen(claw.expel())
+                    .andThen(() -> this.notePosition = NotePosition.SHOOTER)
+                    .andThen(Commands.waitSeconds(1)) // TODO: Change this to a wait until the note is in the shooter?
+                    .andThen(() -> this.notePosition = NotePosition.NONE)
                     .andThen(stopAllMotors());
-
-            this.notePosition = NotePosition.SHOOTER;
-            //TODO: Then once we are done shooting, we want the variable to be set to NONE
         }
 
         return shoot;
     }
 
-    public Command clawToTrap() {
+    public Command noteToTrap() {
         Command shoot = emptyCommand;
 
         if ( this.notePosition == NotePosition.CLAW ) {
             // maybe make setPosition a command ORR Make the Elevator Command 
-            shoot = Commands.runOnce(
-                        () -> elevator.setPosition(TrapConstants.TRAP_POS)
-            ).andThen(
-                Commands.waitUntil(elevator.isAtTargetPosition())
-            ).andThen(claw.expel())
-            .andThen(new WaitCommand(1))
-            .andThen(stopAllMotors());
+            shoot = 
+            Commands.runOnce( 
+                () -> elevator.setPosition(TrapConstants.TRAP_POS) )
+                .andThen(
+                    Commands.waitUntil(elevator.isAtTargetPosition())
+                ).andThen(claw.expel())
+                .andThen(new WaitCommand(1))
+                .andThen(stopAllMotors());
 
             this.notePosition = NotePosition.CLAW;