ElevatorSequenceCommand.java

package frc.robot.commands;

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

/**
 * An autonomous command that sequentially raises the elevator to each level
 * and then returns it to the initial position.
 */
public class ElevatorSequenceCommand extends SequentialCommandGroup {
    
    /**
     * Creates a new ElevatorSequenceCommand.
     * This command will:
     * 1. Move the elevator to LOW
     * 2. Move the elevator to MED
     * 3. Move the elevator to HIGH
     * 4. Move the elevator to EXTRA_HIGH
     * 5. Return to the initial position (LOW)
     * 
     * @param elevator The elevator subsystem to control
     */
    public ElevatorSequenceCommand(Elevator elevator) {
        // Set the subsystem requirements
        addRequirements(elevator);
        
        // Define delay between movements to make it move slowly
        final double DELAY_SECONDS = 2.0;
        
        // Add commands to the sequence
        addCommands(
            // Move to LOW level first
            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.LOW)),
            new WaitCommand(DELAY_SECONDS),
            
            // Move to MED level
            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.MED)),
            new WaitCommand(DELAY_SECONDS),
            
            // Move to HIGH level
            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.HIGH)),
            new WaitCommand(DELAY_SECONDS),
            
            // Move to EXTRA_HIGH level
            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.EXTRA_HIGH)),
            new WaitCommand(DELAY_SECONDS),
            
            // Return to initial position (LOW)
            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.LOW))
        );
    }
}