wiki:ControlSystems/SoftwareTeam/Training/GettingStarted/RobotFSM1

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

The minimum 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.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 Talon l_motor, r_motor;
  private Encoder l_encoder, r_encoder;

  // The possible states for our autonomous behavior
  private enum AutoState { STARTING, DRIVING, TURNING, STOPPED };
  // The current state
  private AutoState state;
  
  private void driveForward() {
    // left and right motors face opposite directions so spin them in
    // opposite directions to turn the wheels in the same direction.
    l_motor.set(0.20);
    r_motor.set(0.20);
    // reset wheel encoders so we can tell how far we've driven
    l_encoder.reset();
    r_encoder.reset();
  }

  private void turnRight() {
    // left and right motors face opposite directions so spin them in
    // the same direction to turn wheels in opposite directions (rotate robot)
    l_motor.set(0.20);
    r_motor.set(-0.20);
    // reset wheel encoders so we can tell how far we've turned
    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() {
    // Talon motor controllers connected to PWM ports 0,1
    l_motor = new Talon(0);
    r_motor = new Talon(1);
    r_motor.setInverted(true); // right motor turns opposite direction
    // Quadrature encoders connected to DIO ports 0,1 and 2,3
    l_encoder = new Encoder(0,1);
    r_encoder = new Encoder(2,3, true); // right wheel turns opposite direction
    // 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() {
  }

  /**
   */
  @Override
  public void autonomousInit() {
    state = AutoState.STARTING;
  }

  /**
   * This function is called periodically during autonomous.
   */
  @Override
  public void autonomousPeriodic() {
    SmartDashboard.putString("Auto state=", state.name());
    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.

Last modified 18 months ago Last modified on Nov 13, 2019, 9:47:08 AM