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.

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.

These demands should be in "units" like degrees or meters: This will make it easy to change and visualize

Key Feature: Minimal Override

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

@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:

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()

@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.