You should review the Java State Machine example before exploring this code.

== State Machine for PWM robot
{{{
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Encoder;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the TimedRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the build.gradle file in the
 * project.
 */
public class Robot extends TimedRobot {
  private static final String kDefaultAuto = "Default";
  private static final String kCustomAuto = "My Auto";
  private String m_autoSelected;
  private final SendableChooser<String> m_chooser = new SendableChooser<>();
  private Talon l_motor, r_motor;
  private Encoder l_encoder, r_encoder;

  private enum AutoState { STARTING, DRIVING, TURNING, STOPPED };
  private AutoState state;
  
  private void driveForward() {
    // to go forward, left and right motors turn in opposite directions
    // so wheels turn in same direction
    l_motor.set(0.20);
    r_motor.set(-0.20);
    // reset encoders so we can tell how far we've driven
    l_encoder.reset();
    r_encoder.reset();
  }

  private void turnRight() {
    // to turn, both motors turn in the same direction
    // so wheels turn in opposite directions
    l_motor.set(0.20);
    r_motor.set(0.20);
    // reset encoders so we can tell how far we've driven
    l_encoder.reset();
    r_encoder.reset();
  }

  private void stop() {
    l_motor.stopMotor();
    r_motor.stopMotor();
  }

  /**
   * This function is run when the robot is first started up and should be
   * used for any initialization code.
   */
  @Override
  public void robotInit() {
    m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
    m_chooser.addOption("My Auto", kCustomAuto);
    SmartDashboard.putData("Auto choices", m_chooser);
    // Talon motor controllers connected to PWM ports 0,1
    l_motor = new Talon(0);
    r_motor = new Talon(1);
    // Quadrature encoders connected to DIO ports 0,1 and 2,3
    l_encoder = new Encoder(0,1);
    r_encoder = new Encoder(2,3);
    // 7.5" wheels with 360-pulse-per-rotation encoders
    l_encoder.setDistancePerPulse((7.5 * 3.141592) / 360);
    r_encoder.setDistancePerPulse((7.5 * 3.141592) / 360);
  }

  /**
   * This function is called every robot packet, no matter the mode. Use
   * this for items like diagnostics that you want ran during disabled,
   * autonomous, teleoperated and test.
   *
   * <p>This runs after the mode specific periodic functions, but before
   * LiveWindow and SmartDashboard integrated updating.
   */
  @Override
  public void robotPeriodic() {
  }

  /**
   * This autonomous (along with the chooser code above) shows how to select
   * between different autonomous modes using the dashboard. The sendable
   * chooser code works with the Java SmartDashboard. If you prefer the
   * LabVIEW Dashboard, remove all of the chooser code and uncomment the
   * getString line to get the auto name from the text box below the Gyro
   *
   * <p>You can add additional auto modes by adding additional comparisons to
   * the switch structure below with additional strings. If using the
   * SendableChooser make sure to add them to the chooser code above as well.
   */
  @Override
  public void autonomousInit() {
    m_autoSelected = m_chooser.getSelected();
    // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
    System.out.println("Auto selected: " + m_autoSelected);
    state = AutoState.STARTING;
  }

  /**
   * This function is called periodically during autonomous.
   */
  @Override
  public void autonomousPeriodic() {
    switch (state) {
      case STARTING:
        // transition to driving state
        driveForward();
        state = AutoState.DRIVING;
        break;
      case DRIVING:
        if ((l_encoder.getDistance() >= 36.0) &&
            (r_encoder.getDistance() >= 36.0)) {
            // if driven desired distance, transition to turning state
            turnRight();
            state = AutoState.TURNING;
        } // else stay in this state until we've gone far enough
        break;
      case TURNING:
        if ((l_encoder.getDistance() >= 3.0) && 
            (r_encoder.getDistance() <= -3.0)) {
            // if turned desired distance, transition to stopped state
            stop();
            state = AutoState.STOPPED;
        } // else stay in this state until turn complete
        break;
      case STOPPED:
        // stay in this state until we exit autonomous
        break;
    }
  }

  /**
   * This function is called periodically during operator control.
   */
  @Override
  public void teleopPeriodic() {
  }

  /**
   * This function is called periodically during test mode.
   */
  @Override
  public void testPeriodic() {
  }
}
}}}