# State-Based Servo Subsystems

We use several variations of a class called `StateBasedServoMotorSubsystem`. These subsystems allow us to control mechanical components by commanding them into **predefined states**, rather than setting raw motor outputs.

Let’s look at **two real examples**:

* `Elevator`: A more complex subsystem that dynamically adjusts its position using scoring distance and an interpolation map.
* `IntakeDeploy`: A simpler subsystem that just goes to fixed positions using a CANcoder.

Both rely on the same state-based pattern—but they diverge significantly in how they implement `writePeriodicOutputs()`.

***

#### Example 1: `IntakeDeploy`  FixedState

The `IntakeDeploy` class is a simple and robust implementation of a servo subsystem.

```java
class IntakeDeploy extends StateBasedServoMotorSubsystemWithCancoder<IntakeDeploy.State>
```

It:

* Uses a **CANcoder** for absolute positioning.
* Moves to **predefined angles** (e.g., `GROUND`, `STOW`, `HUMAN`).
* Uses **MotionMagic** to achieve smooth transitions.

Each state has a hardcoded `demand` (like `-141`) and an allowable error window.

{% hint style="info" %}
These demands should be in "units" like degrees or meters: This will make it easy to change and visualize
{% endhint %}

**Key Feature: Minimal Override**

`IntakeDeploy` doesn't override `writePeriodicOutputs()` at all. That’s because **no extra logic is needed**—the base class handles everything:

```java
@Override
public void outputTelemetry() {
    RobotVisualizer.updateIntakeAngle(getPosition());
    super.outputTelemetry();
}
```

This makes `IntakeDeploy` a great example of **how simple these classes can be when all you need is: "go to a position and stop."**

***

#### Example 2: `Elevator`  Context-Aware Positioning

`Elevator` is built on the same architecture:

```java
class Elevator extends StateBasedServoMotorSubsystem<Elevator.State>
```

But its behavior is more dynamic. It:

* Uses interpolation maps to adjust state positions depending on **distance from the scoring node**.
* Adds a **manual scoring offset** (controlled by the driver).
* Dynamically recalculates the final target before sending it to the motor controller.

**Key Feature: Custom `writePeriodicOutputs()`**

```java
@Override
public void writePeriodicOutputs() {
    double trackedOutput = mState.getTrackedOutput(distanceFromScoringPosition);
    
    if (mState == State.L1 || mState == State.L2 || mState == State.L3 || mState == State.L4)
        trackedOutput += scoringOffset;

    if (mControlState == ControlState.MOTION_MAGIC)
        setSetpointMotionMagic(trackedOutput);

    super.writePeriodicOutputs();
}
```

This method:

1. Gets the **base state demand**.
2. Adjusts it using the interpolating map (`getTrackedOutput()`).
3. Adds a manual offset if necessary.
4. Passes the final target to the motor.

This makes `Elevator` a good example of when **additional logic is required in the periodic control loop**.