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

The most basic requirement for scoring points in autonomous mode is usually to drive off the starting position some distance and then stop. We explored how to do this in the Autonomous and Closed Loop Control examples. However, to do something interesting (and earn more points), robots usually need to perform a sequence of maneuvers and actions. There are several ways to do this, but the simplest is with a state machine.

The example below is the full Robot.java program to drive straight for a set distance and then execute a turn.

State Machine for PWM robot (Macadamia)

/*----------------------------------------------------------------------------*/
/* 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() {
  }
}

Note that:

• The robot is always in one of the defined states: Starting, Driving, Turning, and Stopped.
• The robot transitions from one state to the next by starting a new activity: driveForward, turnRight, stop.

Extra Credit

Modify the program to add more states: for example, after turning, drive forward again.