> For the complete documentation index, see [llms.txt](https://docs.raidrobotics.org/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://docs.raidrobotics.org/commands-v3-state-machine/guide/superstructure.md).

# The superstructure state machine

The **superstructure** is every mechanism except the drivetrain, run together by one state machine. It lives in `superstructure/Superstructure.java`, and it's the part of the project you'll change most during a season.

A state machine models the robot's behavior as a set of **states** (what the robot is doing) and **transitions** (when it moves from one state to another). At any moment the robot is in exactly one state. The superstructure uses the `StateMachine` command built into Commands v3; see WPILib's [State Machines with Commands](https://docs.wpilib.org/en/latest/docs/software/commandbased/commands-v3/state-machines.html) for the full API.

## The states

```mermaid
stateDiagram-v2
    [*] --> IDLE
    IDLE --> INTAKING: intake requested (no piece)
    IDLE --> HOLDING: a piece shows up (a preload)
    INTAKING --> HOLDING: piece is in
    INTAKING --> IDLE: intake request ends
    HOLDING --> SCORING: score requested, and in place
    HOLDING --> IDLE: piece fell out
    SCORING --> IDLE: piece is out
```

| State        | Mechanism 1             | Mechanism 2    | Leaves when...                                                                                  |
| ------------ | ----------------------- | -------------- | ----------------------------------------------------------------------------------------------- |
| **IDLE**     | Stowed                  | Stopped        | Intake is requested with no piece (→ INTAKING), or a piece appears (→ HOLDING)                  |
| **INTAKING** | At the intake position  | Pulling in     | A piece is in (→ HOLDING), or the intake request ends (→ IDLE)                                  |
| **HOLDING**  | At the scoring position | Gently holding | Score is requested **and** Mechanism 1 is in place (→ SCORING), or the piece falls out (→ IDLE) |
| **SCORING**  | At the scoring position | Ejecting       | The eject finishes (→ IDLE)                                                                     |

HOLDING carries the piece at the scoring position, so scoring can start the moment the driver asks.

## How the pieces fit

```mermaid
flowchart LR
    T[Driver's buttons<br/>DriveTeleop] --> R[REQUESTS<br/>&quot;I want to intake&quot;]
    A[Auto routine<br/>ExampleAuto] --> R
    R --> SM[STATE MACHINE<br/>&quot;what the robot does&quot;]
    SM --> C[Mechanism commands<br/>Mechanism1.goTo, Mechanism2.intake]
    C --> M[Motors]
```

* **Requests** are what someone wants: `requestIntake()`, `requestScore()`. Teleop binds them to buttons; autos call them in a sequence. Nothing else ever tells a mechanism what to do. See [Requests](https://docs.raidrobotics.org/commands-v3-state-machine/concepts/requests.md).
* **A state** is one command that tells *every* mechanism what to do. While the robot is in a state, that command runs.
* **A transition** says when to leave a state.

## Defining the state machine

The state machine is built in `createStateMachine()`. It has three parts: the states, the transitions, and the logging hooks.

{% code title="Superstructure.java" overflow="wrap" lineNumbers="true" %}

```java
public StateMachine createStateMachine() {
    final var machine = new StateMachine("Superstructure");

    /* ---- States: what EVERY mechanism does in each state ---- */

    final var idle = machine.addState(Command.parallel(
        mechanism1.goTo("Stow", Mechanism1.STOW),
        mechanism2.stop()
    ).named("Superstructure.Idle"));

    // Ends by itself when the piece is in (see intaking's whenComplete transition)
    final var intaking = machine.addState(Command.parallel(
        mechanism1.goTo("Intake", Mechanism1.INTAKE),
        mechanism2.intake()
    ).until(this::hasGamePiece).named("Superstructure.Intaking"));

    // Carry the piece at the scoring position, so scoring can start right away
    final var holding = machine.addState(Command.parallel(
        mechanism1.goTo("Score", Mechanism1.SCORE),
        mechanism2.hold()
    ).named("Superstructure.Holding"));

    // Only eject() has to finish for the state to be done; mechanism1 just keeps holding.
    final var scoring = machine.addState(Command.parallel(mechanism2.eject())
        .optional(mechanism1.goTo("Score", Mechanism1.SCORE))
        .named("Superstructure.Scoring"));

    machine.setInitialState(idle);

    /* ---- Transitions: when to move from one state to another ---- */

    idle.switchTo(intaking).when(() -> wantIntake && !hasGamePiece());
    idle.switchTo(holding).when(this::hasGamePiece);

    intaking.switchTo(holding).whenComplete();
    intaking.switchTo(idle).when(() -> !wantIntake);

    // "wantScore" alone isn't enough: wait until the mechanism is actually in place
    holding.switchTo(scoring).when(() -> wantScore && mechanism1.isAt(Mechanism1.SCORE));
    holding.switchTo(idle).when(() -> !hasGamePiece());

    scoring.switchTo(idle).whenComplete();

    /* ---- LOGGING: remember and record every state change ---- */

    idle.onEnter(() -> enter(State.IDLE));
    intaking.onEnter(() -> enter(State.INTAKING));
    holding.onEnter(() -> enter(State.HOLDING));
    scoring.onEnter(() -> enter(State.SCORING));

    return machine;
}
```

{% endcode %}

### States

Each call to `machine.addState(command)` adds a state that runs that command. The command is scheduled when the state is entered and canceled when the state is left.

In the template, every state's command is a `Command.parallel(...)` with **one command for every mechanism**. That's the most important rule in this file: a state says what *every* mechanism does, even if the answer is "stay stowed" or "stop". A state that leaves a mechanism out leaves it with no command, so nothing in the code is deciding what it does, and whatever it was last told is a leftover from the previous state.

{% hint style="info" %}
Calling `setInitialState()` is required. The state machine starts in that state every time it's scheduled, which in this project is every time the robot is enabled.
{% endhint %}

### Transitions

The template uses two kinds of transition:

| Transition                      | Fires...                                    | Used for                                                                             |
| ------------------------------- | ------------------------------------------- | ------------------------------------------------------------------------------------ |
| `a.switchTo(b).when(condition)` | the moment the condition **becomes** true   | Requests and sensors: "intake was requested", "the piece fell out"                   |
| `a.switchTo(b).whenComplete()`  | when the state's command finishes by itself | Jobs that end: INTAKING ends once a piece is in, SCORING ends once `eject()` is done |

A few rules to keep in mind:

* `when(...)` reacts to the condition *changing* to true, not to it *being* true. Write conditions that describe the moment you want to leave.
* If two transitions out of a state fire in the same loop, the one declared **first** wins. Put the more important one first.
* A state whose command finishes, with no `whenComplete()` transition, ends the whole state machine. In the template, only INTAKING and SCORING can finish, and both have one.

### Combining a request with a check

The HOLDING → SCORING transition shows a pattern you'll use a lot:

```java
holding.switchTo(scoring).when(() -> wantScore && mechanism1.isAt(Mechanism1.SCORE));
```

`wantScore` alone isn't enough. The driver can hold the score button early, and the robot starts scoring as soon as Mechanism 1 actually arrives. The same idea works for any "only when it's safe" rule: shooter up to speed, elevator high enough, aimed at the target.

### Ending a state by itself

INTAKING's command ends itself with `.until(this::hasGamePiece)`. Its `whenComplete()` transition then moves to HOLDING. SCORING's command ends when `eject()` finishes, because `eject()` is the only **required** command in its parallel group. Mechanism 1's `goTo` is added with `.optional(...)`, so it holds position without keeping the state open.

## The current state

The `StateMachine` knows which state it's in, but the rest of the code needs to know too, for `readyToScore()`, for the auto requests, and for logging. So each state has an `onEnter` hook that records it in an ordinary `State` enum:

```java
public enum State { IDLE, INTAKING, HOLDING, SCORING }

private void enter(State next) {
    if (next != state) {    // (the state machine starting up in IDLE isn't a change)
        RobotLog.event("Superstructure: " + state + " -> " + next);
    }
    state = next;
}
```

Other code reads it with `getState()`. Every change is written to the log as an event, and `periodic()` logs `Superstructure/State` every loop.

{% hint style="warning" %}
RAIDLogs reads `Superstructure/State` by name to say what the robot was doing when something went wrong ("brownout while SCORING"). Don't rename the table, the entry, or the class.
{% endhint %}

## Who runs the state machine

The superstructure doesn't run its own state machine. Each opmode builds one with `createStateMachine()` and runs it while that opmode is enabled:

{% code title="DriveTeleop.java" %}

```java
RobotModeTriggers.teleop().whileTrue(superstructure.createStateMachine());
```

{% endcode %}

`ExampleAuto` does the same with `RobotModeTriggers.autonomous()`. So the state machine starts over from IDLE every time the robot is enabled, and in an opmode that doesn't start one (like `DoNothingAuto`), no mechanism moves.

The state machine itself has no requirements. The active state's command owns the mechanisms, so a single state can command several mechanisms at once.

## Readings

The superstructure answers a few questions for other code:

| Method           | Returns                                                    |
| ---------------- | ---------------------------------------------------------- |
| `getState()`     | The current `State`                                        |
| `hasGamePiece()` | Whether Mechanism 2's sensor sees a piece                  |
| `readyToScore()` | `true` in HOLDING with Mechanism 1 at the scoring position |

## Rules between mechanisms

When two mechanisms can collide ("the arm may not go down while the elevator is low"), the rule goes in the superstructure, not in either mechanism. Wrap the targets in a "safe" function that every state uses. `SimpleSuperstructure-v3`'s `Superstructure` has a worked example (`safeHeight` and `safeAngle`).

## The side-view drawing

`periodic()` also updates a `Mechanism2d` drawing, logged as `Superstructure/Drawing`: a bar across the top colored by state, Mechanism 1 as a line that pivots, and a white tip while there's a game piece. It works on the real robot too. When the real mechanisms are built, redraw it with a root per mechanism.

| State    | Color |
| -------- | ----- |
| IDLE     | Gray  |
| INTAKING | Green |
| HOLDING  | Blue  |
| SCORING  | Red   |

## Changing the state machine

The most common change in a season is adding a state. The steps are in [Add a state](https://docs.raidrobotics.org/commands-v3-state-machine/how-to/add-a-state.md). Adding a mechanism to the superstructure is in [Add a mechanism](https://docs.raidrobotics.org/commands-v3-state-machine/how-to/add-a-mechanism.md).


---

# Agent Instructions
This documentation is published with GitBook. GitBook is the documentation platform designed so that both humans and AI agents can read, navigate, and reason over technical content effectively. Learn more at gitbook.com.

## Querying This Documentation
If you need additional information that is not directly available in this page, you can query the documentation by asking a question.

Perform an HTTP GET request on the following URL with the `ask` and `goal` query parameters:

```
GET https://docs.raidrobotics.org/commands-v3-state-machine/guide/superstructure.md?ask=<question>&goal=<user_goal>
```

`ask` is the immediate question: it should be specific, self-contained, and written in natural language.
`goal` is what the user is ultimately trying to achieve, the reason they need the answer. Sharing it helps GitBook give you a better, more relevant answer. A goal is most helpful when it describes the outcome the user wants rather than restating the question. For example, with `ask=how do I create an API token`, a goal like `automate deployments from our CI pipeline` lets GitBook tailor the answer to that use case.

The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.
