feat: clean up staging and shoot commands

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -16,6 +16,8 @@
 import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
 import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.commands.indexer.IntakeCommand;
+import org.team1540.robot2024.commands.shooter.ShootSequence;
+import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
 import org.team1540.robot2024.subsystems.drive.*;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
@@ -65,8 +67,6 @@ public class RobotContainer {
     public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
 
     // TODO: testing dashboard inputs, remove for comp
-    public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
-    public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -183,7 +183,7 @@ private void configureButtonBindings() {
                 ).ignoringDisable(true)
         );
 
-        copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
+        copilot.a().onTrue(new ShootSequence(shooter, indexer))
                 .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));
 
         driver.a().onTrue(new IntakeCommand(indexer));

src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java

@@ -0,0 +1,35 @@
+package org.team1540.robot2024.commands.indexer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.tramp.Tramp;
+
+public class StageTrampCommand extends Command {
+
+    private final Tramp tramp;
+    private final Indexer indexer;
+
+    public StageTrampCommand(Tramp tramp, Indexer indexer) {
+        this.tramp = tramp;
+        this.indexer = indexer;
+        addRequirements(tramp, indexer);
+    }
+
+    @Override
+    public void initialize() {
+        tramp.setPercent(0.5);
+        indexer.setFeederVelocity(-600);
+        indexer.setIntakePercent(0.5);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return tramp.isBeamBreakBlocked();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexer.stopFeeder();
+        tramp.stop();
+    }
+}

src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java

@@ -0,0 +1,25 @@
+package org.team1540.robot2024.commands.shooter;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+
+class PrepareShooterCommand extends Command {
+    private final Shooter shooter;
+
+    public PrepareShooterCommand(Shooter shooter) {
+        this.shooter = shooter;
+        addRequirements(shooter);
+    }
+    @Override
+    public void execute() {
+        // TODO: Make this dynamically update based on estimated pose
+        shooter.setFlywheelSpeeds(1000, 1000);
+        shooter.setPivotPosition(Rotation2d.fromDegrees(30));
+    }
+
+    @Override
+    public boolean isFinished() {
+        return shooter.areFlywheelsSpunUp() && shooter.isPivotAtSetpoint();
+    }
+}

src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java

@@ -0,0 +1,26 @@
+package org.team1540.robot2024.commands.shooter;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+
+public class ShootSequence extends SequentialCommandGroup {
+    public ShootSequence(Shooter shooter, Indexer indexer) {
+        addCommands(
+                Commands.parallel(
+                        indexer.feedToShooter(),
+                        new PrepareShooterCommand(shooter)
+                ),
+                Commands.runOnce(() -> indexer.setIntakePercent(1), indexer)
+                // TODO: Add a wait for having completed the shot (steady then current spike/velocity dip and then back down?)
+        );
+    }
+
+
+    @Override
+    public InterruptionBehavior getInterruptionBehavior() {
+        return InterruptionBehavior.kCancelIncoming;
+    }
+}

src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java

@@ -0,0 +1,28 @@
+package org.team1540.robot2024.commands.shooter;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+
+public class TuneShooterCommand extends Command {
+    private final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
+    private final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
+    private final LoggedDashboardNumber angleSetpoint = new LoggedDashboardNumber("Shooter/Pivot/angleSetpoint", 30);
+    private final Shooter shooter;
+
+    public TuneShooterCommand(Shooter shooter) {
+        this.shooter = shooter;
+        addRequirements(shooter);
+    }
+
+    public void execute() {
+        shooter.setFlywheelSpeeds(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get());
+        shooter.setPivotPosition(Rotation2d.fromDegrees(angleSetpoint.get()));
+    }
+
+    public void end() {
+        shooter.stopFlywheels();
+        shooter.stopPivot();
+    }
+}

src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java

@@ -11,42 +11,53 @@
 
 
 public class Indexer extends SubsystemBase {
-    private final IndexerIO indexerIO;
-    private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged();
+    private final IndexerIO io;
+    private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged();
     private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP);
     private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI);
     private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD);
 
 
-    public Indexer(IndexerIO indexerIO) {
-        this.indexerIO = indexerIO;
+    public Indexer(IndexerIO io) {
+        this.io = io;
     }
 
     public void periodic() {
-        indexerIO.updateInputs(indexerInputs);
-        Logger.processInputs("Indexer", indexerInputs);
+        io.updateInputs(inputs);
+        Logger.processInputs("Indexer", inputs);
         if (tuningMode) {
             if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
-                indexerIO.configureFeederPID(kP.get(), kI.get(), kD.get());
+                io.configureFeederPID(kP.get(), kI.get(), kD.get());
             }
         }
     }
 
     public void setIntakePercent(double percent) {
-        indexerIO.setIntakeVoltage(percent * 12.0);
+        io.setIntakeVoltage(percent * 12.0);
     }
 
     public boolean isNotePresent() {
-        return indexerInputs.noteInIntake;
+        return inputs.noteInIntake;
+    }
+
+    public void setFeederVelocity(double setpointRPM) {
+        io.setFeederVelocity(setpointRPM);
     }
 
     public Command feedToAmp() {
-        return Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this);
+        return Commands.runOnce(() -> io.setFeederVelocity(-600), this);
     }
 
     public Command feedToShooter() {
-        return Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this);
+        return Commands.runOnce(() -> io.setFeederVelocity(1200), this);
     }
 
+    // TODO: Add method to check if feeder is spun up
 
+    public void stopFeeder() {
+        io.setFeederVoltage(0);
+    }
+    public void stopIntake() {
+        io.setIntakeVoltage(0);
+    }
 }

src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java

@@ -9,6 +9,8 @@
 import org.team1540.robot2024.util.LoggedTunableNumber;
 import org.team1540.robot2024.util.math.AverageFilter;
 
+import java.util.function.Supplier;
+
 import static org.team1540.robot2024.Constants.Shooter.Flywheels;
 import static org.team1540.robot2024.Constants.Shooter.Pivot;
 
@@ -161,9 +163,9 @@ public boolean isPivotAtSetpoint() {
                 pivotSetpoint.getRotations(), pivotPositionFilter.getAverage(), Pivot.ERROR_TOLERANCE.getRotations());
     }
 
-    public Command spinUpCommand(double leftSetpoint, double rightSetpoint) {
+    public Command spinUpCommand(Supplier<Double> leftSetpoint, Supplier<Double> rightSetpoint) {
         return new FunctionalCommand(
-                () -> setFlywheelSpeeds(leftSetpoint, rightSetpoint),
+                () -> setFlywheelSpeeds(leftSetpoint.get(), rightSetpoint.get()),
                 () -> {},
                 (ignored) -> {},
                 this::areFlywheelsSpunUp,
@@ -178,4 +180,5 @@ public Command setPivotPositionCommand(Rotation2d setpoint) {
                 this::isPivotAtSetpoint,
                 this);
     }
+
 }

src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java

@@ -25,7 +25,7 @@ public void periodic() {
         Logger.processInputs("Tramp", inputs);
     }
 
-    public void stopTramp() {
+    public void stop() {
         io.setVoltage(0);
     }