factor: moved FunctionalCommand to an indexer method

flamingchickens1540 · Feb 7, 2024 · ed529d0 · ed529d0
1 parent 95fbd9f
commit ed529d0
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 9 deletions.
diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java
@@ -1,7 +1,6 @@
 package org.team1540.robot2024.commands.shooter;
 
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import org.team1540.robot2024.commands.indexer.PrepareFeederForShooter;
 import org.team1540.robot2024.subsystems.indexer.Indexer;
@@ -13,13 +12,7 @@ public ShootSequence(Shooter shooter, Indexer indexer) {
                 Commands.parallel(
                         new PrepareShooterCommand(shooter),
                         Commands.sequence(
-                                new FunctionalCommand(
-                                        () -> indexer.setIntakePercent(-1),
-                                        () -> {},
-                                        (interrupted) -> indexer.stopIntake(),
-                                        () -> !indexer.isNoteStaged(),
-                                        indexer
-                                ),
+                                indexer.moveNoteOut(),
                                 new PrepareFeederForShooter(indexer)
                         )
                 ),

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.Constants;
@@ -51,7 +52,16 @@ public Command feedToAmp() {
     public Command feedToShooter() {
         return Commands.runOnce(() -> io.setFeederVelocity(1200), this);
     }
-
+
+    public Command moveNoteOut() {
+        return new FunctionalCommand(
+                () -> setIntakePercent(-1),
+                () -> {},
+                (interrupted) -> stopIntake(),
+                () -> !isNoteStaged(),
+                this
+        );
+    }
     public boolean isFeederAtSetpoint() {
         return Math.abs(inputs.feederVelocityError) < VELOCITY_ERR_TOLERANCE_RPM;
 //        return MathUtil.isNear(inputs.setpointRPM, inputs.feederVelocityRPM, VELOCITY_ERR_TOLERANCE_RPM);