FetchCoralFromStationCommand.java

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import frc.robot.ElevatorLevel;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.CoralClaw;

/**
 * A command that moves the elevator to STATION level and then activates 
 * the coral claw intake to fetch a coral from the station.
 */
public class FetchCoralFromStationCommand extends SequentialCommandGroup {
  /**
   * Creates a new FetchCoralFromStationCommand.
   *
   * @param elevator The elevator subsystem
   * @param coralClaw The coral claw subsystem
   */
  public FetchCoralFromStationCommand(Elevator elevator, CoralClaw coralClaw) {
    addRequirements(elevator, coralClaw);
    
    addCommands(
      // Move the elevator to STATION level
      new InstantCommand(() -> elevator.setTarget(ElevatorLevel.STATION), elevator),
      
      // Wait until the elevator reaches the target position
      new WaitUntilCommand(elevator::isAtTarget),
      
      // Activate coral claw intake
      new InstantCommand(coralClaw::intake, coralClaw)
    );
  }
}