Autonomous

1. Create a New Auto Class

PathPlanner

Before you begin writing any code for autonomous use PathPlanner to brainstorm and design your path.

Communication with the strategy team is important here (If you can convince them to design the strategy that would be optimal, in the past they just greenlight/suggest changes)

2. Create a New Auto Class

Start by creating a new class in the com.team5817.frc2025.autos.Modes package. This class must extend AutoBase and implement the routine() method.

Example:

public class MyAutoMode extends AutoBase {
    private Superstructure s = Superstructure.getInstance();
    private TrajectorySet t;

    public MyAutoMode(StartingPosition startingPosition) {
        boolean mirror = startingPosition.mirrored;

        Trajectory part1 = l.trajectories.get(startingPosition.name + "To_Start");
        Trajectory part2 = l.trajectories.get("StartToPickup");
        Trajectory part3 = l.trajectories.get("PickupToScore");

        t = new TrajectorySet(
            mirror,
            part1,
            part2,
            part3
        );
    }

    @Override
    public void routine() {
        // Autonomous sequence goes here
    }
}

---

3. Define and Use TrajectorySet

TrajectorySet lets you define an ordered list of trajectories that your autonomous routine will use. You retrieve the next path using t.next() in the order you added them.

t = new TrajectorySet(
    mirror,
    l.trajectories.get("Path1"),
    l.trajectories.get("Path2"),
    ...
);

Each call to t.next() advances to the next trajectory in the sequence.

---

4. Implement the routine() Method

The routine() method defines the steps the robot takes during autonomous mode. Use the r() method to queue actions, either sequentially or in parallel.

@Override
public void routine() {
    if (RobotMode.mode == RobotMode.Mode.SIM) {
        mSim.setSimulationWorldPose(t.initalPose().wpi());
    }

    s.setReadyToScore(false);

    // Drive to first location and raise elevator
    r(new ParallelAction(List.of(
        new TrajectoryAction(t.next(), 2),
        new LambdaAction(() -> s.setGoal(GoalState.L4))
    )));

    r(new WaitAction(0.5));
    s.setReadyToScore(true);

    r(new WaitForSuperstructureAction());
    r(new WaitAction(AutoConstants.coralSpit));

    // Continue to next segment
    r(new TrajectoryAction(t.next()));
    ...
}

---

5. Coordinating Subsystems with Path Following

Use ParallelAction and SequentialAction to run multiple actions at the same time or in a specific order.

Example: Change Superstructure State After Reaching a Distance

r(new ParallelAction(List.of(
    new TrajectoryAction(t.next(), 0.4),
    new SequentialAction(List.of(
        new WaitToPassDistanceToReef(AutoConstants.exitDistance),
        new LambdaAction(() -> s.setGoal(GoalState.A1))
    ))
)));

---

6. Reuse Behavior with Helper Methods

Common operations, such as scoring or stowing, should be wrapped in their own methods to keep your routine concise and maintainable.

Example: scoreNet()

public void scoreNet() {
    s.setReadyToScore(false);
    r(new ParallelAction(List.of(
        new TrajectoryAction(t.next()),
        new SequentialAction(List.of(
            new WaitToPassDistanceToReef(AutoConstants.exitDistance),
            new LambdaAction(() -> s.setGoal(GoalState.SUPER_NET))
        ))
    )));
    r(new WaitAction(0.2));
    s.setReadyToScore(true);
    r(new WaitAction(0.3));
    s.setGoal(GoalState.STOW);
}

---

7. Simulation Pose Setup

To enable simulation accurately, set the robot's initial pose using the first trajectory's pose.

if (RobotMode.mode == RobotMode.Mode.SIM) {
    mSim.setSimulationWorldPose(t.initalPose().wpi());
}

8. Add to Auto Selector

We do not have a general solution for this yet, in 2025 we had a complex selector that allowed to select an auto dynamically, specify intake and scoring locations and it would stitch the auto together accordingly, I would really recommed checking it out!