public class RobotContainer {
    // 1. Define Subsystems (The Hardware)
    private final DriveSubsystem m_drive = new DriveSubsystem();
    private final ArmSubsystem m_arm = new ArmSubsystem();
 
    // 2. Define Controllers (The Input)
    private final CommandXboxController m_driver = new CommandXboxController(0);
 
    public RobotContainer() {
        // 3. Configure bindings
        configureBindings();
 
        // 4. Set Default Commands (What to do when no buttons are pressed)
        m_drive.setDefaultCommand(
            // Run command that constantly reads joystick
            new RunCommand(
                () -> m_drive.arcadeDrive(-m_driver.getLeftY(), -m_driver.getRightX()),
                m_drive
            )
        );
    }
 
    private void configureBindings() {
        // "When A is pressed, print Hello"
        m_driver.a().onTrue(new InstantCommand(() -> System.out.println("Hello!")));
    }
}

public class DriveDistanceCommand extends Command {
    private final DriveSubsystem m_drive;
    private final double m_distance;
 
    // Constructor: Require the subsystem!
    public DriveDistanceCommand(DriveSubsystem drive, double distance) {
        m_drive = drive;
        m_distance = distance;
        addRequirements(m_drive); // Crucial! Tells scheduler "I need the drivebase"
    }
 
    @Override
    public void initialize() {
        m_drive.resetEncoders();
    }
 
    @Override
    public void execute() {
        m_drive.tankDrive(0.5, 0.5); // Drive forward at 50%
    }
 
    @Override
    public boolean isFinished() {
        // Stop if we passed the distance
        return m_drive.getAverageDistance() >= m_distance;
    }
 
    @Override
    public void end(boolean interrupted) {
        m_drive.stop(); // Safety first!
    }
}

// In RobotContainer.configureBindings()
 
// While holding 'X', spin intake. When released, it stops (because command ends).
m_driver.x().whileTrue(new StartIntakeCommand(m_intake));
 
// When 'A' is pressed, schedule the "Score" command. It runs until it finishes itself.
m_driver.a().onTrue(new ScoreAmpCommand(m_arm, m_shooter));
 
// Triggers can be anything! Not just buttons!
// "If battery voltage drops below 10V, rumble controller"
new Trigger(() -> RobotController.getBatteryVoltage() < 10)
    .onTrue(new InstantCommand(() -> m_driver.getHID().setRumble(RumbleType.kBothRumble, 1)));