StatePOC.java

public class StatePOC extends LinearOpMode {
  private StateMachine autoMachine;
  private StateMachine wobbleMachine;

  enum WobbleState {
    IDLE,
    GRAB,
    LIFT,
    HOLD,
    DUMP,
  }

  enum AutoState {
    IDLE,
    GO_TO_POINT_1,
    GO_TO_POINT_2,
    GRAB_WOBBLE,
    GO_TO_POINT_3,
    DUMP_WOBBLE
  }

  public void runOpMode() {
     SampleMecanumDrive drive = new SampleMecanumDrive();

     // Do your trajectory stuff
     
     autoMachine = new StateMachineBuilder<AutoState>()
        .state(IDLE)
        .waitForStart()

        .state(GO_TO_POINT_1)
        .onEnter(() -> drive.followTrajectoryAsync(traj1)) // followTrajectory upon entering state
        .transition(() -> !drive.isBusy())                 // Transition when trajectory is finished

        .state(GO_TO_POINT_2)
        .onEnter(() -> drive.followTrajectoryAsync(traj2)) // followTrajectory upon entering state
        .transition(() -> !drive.isBusy())                 // Transition when trajectory is finished

        .state(GRAB_WOBBLE)
        .onEnter(() -> wobbleMachine.start())               // Start the wobble machine upon entering state
        .transition(() -> wobbleMachine.getState() == HOLD) // We wait until wobble machine is on HOLD

        .state(GO_TO_POINT_3)
        .onEnter(() -> drive.followTrajectoryAsync(traj3)) // followTrajectory upon entering state
        .transition(() -> !drive.isBusy())                 // Transition when trajectory is finished

        .state(DUMP_WOBBLE)
        .transition(() -> wobbleMachine.transition()) // Force wobble machine to transition to next state

        .exit(IDLE) // Exit to an IDLE state

        .build()

      wobbleMachine = new StateMachineBuilder<WobbleState>()
          .state(IDLE)
          .waitForStart()

          .state(GRAB)
          .onEnter(() -> servo1.setPosition(0.5)) // Set position on enter
          .transitionTimed(0.3)                   // Transition to next state after 0.3 seconds

          .state(LIFT)
          .onEnter(() -> servo2.setPosition(0.5)) // Set position on enter
          .transitionTimed(0.5)                   // Transition to next state after 0.5 seconds

          .state(HOLD) // HOLD state doesn't have a transition
                       // state machine will not transition unless transition() is called externally
          
          .state(DUMP)
          .onEnter(() -> servo1.setPosition(0.5)) // Set position on enter
          .transitionTimed(0.3)                   // Transition to next state after 0.3 seconds
          .onExit(() -> servo2.setPosition(0.3))  // Set position on exit

          .exit(IDLE) // Exit to an IDLE state

          .build();

    waitForStart()

    autoMachine.start();

    while(opModeIsActive()) {
      drive.update();

      autoMachine.update();
      wobbleMachine.update();
    }
  }
}