==================
Command-Based Programming: Intake Example
==================
.. contents:: 
   :local:
   :depth: 2

----

Command-Based Programming: Intake Example
==================

In previous sections, we explored **time-based programming**, where robot actions were controlled using delays and sequential logic.

While that approach works for simple behaviors, it becomes difficult to scale as robots grow more complex.

This tutorial introduces **command-based programming**, a structured and modular approach used in FRC to manage robot behavior more effectively.

---

Why Command-Based?
------------------

Instead of writing one long sequence of actions, command-based programming separates your robot into:

- **Subsystems** → represent hardware and capabilities
- **Commands** → represent actions over time
- **RobotContainer** → connects controls to commands

Think of it like this:

- Subsystem = *what the robot has*
- Command = *what the robot does*

---

The Intake Example
------------------

We will model an intake with two mechanisms:

1. **Arm (Up/Down)**
   - Controlled by **position** (rotations)
   - Uses **closed-loop position control**

2. **Rollers**
   - Controlled by **velocity** (RPM)
   - Uses **closed-loop velocity control**

Even though these are different control loops, they belong to the **same subsystem**.

---

Subsystem Design
----------------

The subsystem represents the hardware and exposes *capabilities*, not full behaviors.

.. code-block:: java

    public class IntakeSubsystem extends SubsystemBase {

        // Motors
        private final CANSparkMax armMotor;
        private final CANSparkMax rollerMotor;

        // Encoders
        private final RelativeEncoder armEncoder;
        private final RelativeEncoder rollerEncoder;

        // PID Controllers
        private final SparkMaxPIDController armPID;
        private final SparkMaxPIDController rollerPID;

        public IntakeSubsystem() {

            armMotor = new CANSparkMax(1, MotorType.kBrushless);
            rollerMotor = new CANSparkMax(2, MotorType.kBrushless);

            armEncoder = armMotor.getEncoder();
            rollerEncoder = rollerMotor.getEncoder();

            armPID = armMotor.getPIDController();
            rollerPID = rollerMotor.getPIDController();

            // Configure PID (example values)
            armPID.setP(0.1);
            rollerPID.setP(0.0002);

            armMotor.setIdleMode(IdleMode.kBrake);
        }

### Arm (Position Control)

.. code-block:: java

        public void setArmPosition(double rotations) {
            armPID.setReference(rotations, ControlType.kPosition);
        }

        public double getArmPosition() {
            return armEncoder.getPosition();
        }

        public boolean isArmAtPosition(double target) {
            return Math.abs(getArmPosition() - target) < 0.2;
        }

### Rollers (Velocity Control)

.. code-block:: java

        public void setRollerVelocity(double rpm) {
            rollerPID.setReference(rpm, ControlType.kVelocity);
        }

        public void stopRollers() {
            rollerMotor.set(0);
        }

        public void stopAll() {
            armMotor.set(0);
            rollerMotor.set(0);
        }
    }

**Key Idea:**
The subsystem does **not** decide *when* to act.  
It only provides the ability to act.

---

Command Design
--------------

Commands define behavior over time.

They answer:
- When does something start?
- When does it stop?
- What happens in between?

---

Deploy Intake Command (Arm Only)
--------------------------------

.. code-block:: java

    public class DeployIntakeCommand extends CommandBase {

        private final IntakeSubsystem intake;
        private final double targetPosition;

        public DeployIntakeCommand(IntakeSubsystem intake, double targetPosition) {
            this.intake = intake;
            this.targetPosition = targetPosition;

            addRequirements(intake);
        }

        @Override
        public void initialize() {
            intake.setArmPosition(targetPosition);
        }

        @Override
        public boolean isFinished() {
            return intake.isArmAtPosition(targetPosition);
        }

        @Override
        public void end(boolean interrupted) {
            intake.stopAll();
        }
    }

**Concepts:**

- ``initialize()`` runs once
- ``isFinished()`` determines when to stop
- PID handles the motion automatically

---

Run Rollers Command
-------------------

.. code-block:: java

    public class RunRollersCommand extends CommandBase {

        private final IntakeSubsystem intake;
        private final double speedRPM;

        public RunRollersCommand(IntakeSubsystem intake, double speedRPM) {
            this.intake = intake;
            this.speedRPM = speedRPM;

            addRequirements(intake);
        }

        @Override
        public void initialize() {
            intake.setRollerVelocity(speedRPM);
        }

        @Override
        public boolean isFinished() {
            return false;
        }

        @Override
        public void end(boolean interrupted) {
            intake.stopRollers();
        }
    }

**Concepts:**

- Runs continuously
- Ends when interrupted (e.g., button release)

---

Combining Commands
------------------

One of the biggest advantages of command-based programming is **composition**.

### Sequential (one after another)

.. code-block:: java

    new DeployIntakeCommand(intake, 10.0)
        .andThen(new RunRollersCommand(intake, 3000));

### Parallel (at the same time)

.. code-block:: java

    new DeployIntakeCommand(intake, 10.0)
        .alongWith(new RunRollersCommand(intake, 3000));

---

Controller Binding
------------------

Commands are triggered by user input in ``RobotContainer``.

.. code-block:: java

    controller.a().whileTrue(
        new RunRollersCommand(intake, 3000)
    );

    controller.b().onTrue(
        new DeployIntakeCommand(intake, 10.0)
    );

    controller.x().whileTrue(
        new DeployIntakeCommand(intake, 10.0)
            .alongWith(new RunRollersCommand(intake, 3000))
    );

---

Subsystem Requirements
----------------------

Each command declares:

.. code-block:: java

    addRequirements(intake);

This ensures:

- Only one command controls the subsystem at a time
- Prevents conflicting motor commands

---

Key Takeaways
-------------

- Subsystems define **capabilities**
- Commands define **behavior over time**
- Closed-loop control lets you set goals instead of power
- Commands can be **combined and reused**

---

Comparison to Time-Based Programming
-------------------------------------

+----------------------+-----------------------------+------------------------------+
| Feature              | Time-Based                  | Command-Based                |
+======================+=============================+==============================+
| Structure            | Linear                      | Modular                      |
+----------------------+-----------------------------+------------------------------+
| Scalability          | Limited                     | High                         |
+----------------------+-----------------------------+------------------------------+
| Reusability          | Low                         | High                         |
+----------------------+-----------------------------+------------------------------+
| Parallel Actions     | Difficult                   | Built-in                     |
+----------------------+-----------------------------+------------------------------+

---

Next Steps
----------

To continue learning, consider:

- Adding **soft limits** to the arm
- Tuning PID values for better control
- Introducing **feedforward** for gravity compensation
- Exploring **default commands** for idle behavior

Command-based programming may feel more complex at first, but it enables cleaner, safer, and more scalable robot code.