Examples

Useful examples of combination with other libraries and common use cases.

A basic full op-mode example is at the end of the essential usage page.

Here is a full op-mode example of a state machine in combination with the Roadrunner Library:

public class SampleSFMachine {

  enum States {
      SEQ1,
      SEQ2
  }
        
  @Override
  public void runOpMode() throws InterruptedException {
        
      SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
      drive.setPoseEstimate(new Pose2d(0,0,0));

      TrajectorySequence sequence1 = drive.trajectorySequenceBuilder(new Pose2d(0,0,0))
          .lineTo(new Vector2d(10, 0))
          .lineTo(new Vector2d(10, 10))
          .build();
        
      TrajectorySequence sequence2 = drive.trajectorySequenceBuilder(new Pose2d(10,10,0)) 
          .lineTo(new Vector2d(0, 10))
          .lineTo(new Vector2d(0, 0))
          .build();

      StateMachine machine = new StateMachineBuilder()
          .state(States.SEQ1)
          .onEnter(() -> drive.followTrajectorySequenceAsync(sequence1))
          .transition(() -> !drive.isBusy())
        
          .state(States.SEQ2)
          .onEnter(() -> drive.followTrajectorySequenceAsync(sequence2))
        
          .build();
                
       waitForStart();
       
       machine.start();
       
       while(opModeIsActive()) {
           drive.update();
           machine.update();
       }
   }
}

Advanced Roadrunner Machine

The machine shown above is relatively basic. It will follow the first trajectory sequence, and when that is done, follow the second. We can make this machine more adaptive by using functions covered in Advanced Usage.

drive = new SampleMecanumDrive(hardwareMap);

drive.setPoseEstimate(new Pose2d(0,0,0));

ElapsedTime runtime = new ElapsedTime();
        
TrajectorySequence sequence1 = drive.trajectorySequenceBuilder(new Pose2d(0,0,0))
        .lineTo(new Vector2d(10, 0))
        .lineTo(new Vector2d(10, 10))
        .build();

TrajectorySequence sequence2 = drive.trajectorySequenceBuilder(new Pose2d(10,10,0))
        .lineTo(new Vector2d(0, 10))
        .lineTo(new Vector2d(0, 0))
        .build();

StateMachine machine = new StateMachineBuilder()
        .state(states.seq1)
        .onEnter(() -> {
                drive.followTrajectorySequenceAsync(sequence1); 
                runtime.reset();
        })
        .transition(() -> !drive.isBusy(), states.seq2)
        .transition(() -> runtime.seconds() > 9 && drive.isBusy(),
                 states.fallback1,
                 () -> telemetry.addLine("Falling back!"))
        
        .state(states.seq2)
        .onEnter(() -> {
                drive.followTrajectorySequenceAsync(sequence2); 
                runtime.reset();
        })
        .transition(() -> runtime.seconds() > 9 && drive.isBusy(),
                states.fallback2,
                () -> telemetry.addLine("Falling back!"))

        .state(states.fallback1, true)
        .onEnter(() -> drive.followTrajectorySequenceAsync(
                drive.trajectorySequenceBuilder(drive.getPoseEstimate())
                        .lineTo(new Vector2d(0, 0))
                        .build()))
        .transition(() -> !drive.isBusy(),
                states.seq1,
                () -> {
                        runtime.reset();
                        telemetry.addLine("Going back to main machine");
                })

        .state(states.fallback2, true)
        .onEnter(() -> drive.followTrajectorySequenceAsync(
                drive.trajectorySequenceBuilder(drive.getPoseEstimate())
                        .lineTo(new Vector2d(10, 10))
                        .build()))
        .transition(() -> !drive.isBusy(),
                states.seq2,
                () -> {
                        runtime.reset();
                        telemetry.addLine("Going back to main machine");
                })
        .build();

This machine will check if more than 9 seconds have elapsed in the state without completing the trajectory, and if the trajectory has not been completed, go back to the start of the trajectory to try again. This use of fallback and pointer states allows for a more adaptive autonomous that will try to compensate for disturbances or errors.

To run a machine that uses Roadrunner, you would simply add drive.update() to the loop that you are calling machine.update() in. This example also uses telemetry, so it would be necessary to add telemetry.update() to the loop as well.

---

Nested State Machine

Another full op-mode example is located at the end of the Nested State Machine section.