Unit 6 · Week 11

State Machines & Pseudocode

How to plan an autonomous routine before writing a single line of C++.

You'll learn

Decomposition, breaking a big task into small, testable pieces.
Pseudocode, writing your plan in English before code.
Finite state machines (FSMs), the standard way to organize autonomous routines.

Decomposition

Look at any autonomous task: "score the green ring on the high goal." That sounds like one thing, but it's at least five:

Drive to the ring.
Lower the intake.
Drive forward slowly to grab the ring.
Drive to the goal.
Raise the arm and release.

Now each step is testable on its own. If step 3 fails, you know exactly which 10 lines of code to look at.

Pseudocode

Pseudocode is your plan in English. Write it before code. It's faster to fix logic mistakes here than after you've debugged for 30 minutes.

// Pseudocode for "score the green ring"
//
// drive forward until distance sensor < 200 mm from ring
// open intake
// drive forward 6 inches at low speed (let intake grab)
// drive backward 12 inches
// turn to face goal (heading = 90°)
// drive forward until distance sensor < 150 mm from goal
// raise arm to scoring position
// open intake to release
// back up 6 inches

Finite State Machines

An FSM is a list of named states, with rules for how to move from one state to the next. Only one state is active at a time. This stops "logic spaghetti", if statements nested inside if statements inside if statements.

enum AutoState {
  DRIVE_TO_RING,
  GRAB_RING,
  DRIVE_TO_GOAL,
  SCORE,
  DONE
};

AutoState state = DRIVE_TO_RING;

while (state != DONE) {
  switch (state) {

    case DRIVE_TO_RING:
      LeftMotor.spin(forward, 50, percent);
      RightMotor.spin(forward, 50, percent);
      if (DistanceSensor.objectDistance(mm) < 200) {
        LeftMotor.stop();
        RightMotor.stop();
        state = GRAB_RING;       // transition
      }
      break;

    case GRAB_RING:
      Intake.spin(forward);
      wait(800, msec);
      state = DRIVE_TO_GOAL;
      break;

    case DRIVE_TO_GOAL:
      // ... turn and drive to goal
      state = SCORE;
      break;

    case SCORE:
      Arm.spinToPosition(450, deg);
      Intake.spin(reverse);
      wait(500, msec);
      state = DONE;
      break;

    case DONE:
      break;
  }
  wait(20, msec);
}

Why this is better: each state is small and reads top-to-bottom. Adding a new step is one new case. If something breaks, you can Brain.Screen.print(state) and immediately know what the robot is doing right now.

Drawing your FSM

Before coding, draw the states as circles and the transitions as arrows. If you can't draw it, you don't understand it yet.

Try it

Pick a real autonomous routine from your team's last match. Write the pseudocode. Then draw the FSM. Then write the C++. Compare to what your team actually had, usually the FSM version is cleaner.

Curated references

Coding Challenge

Build a patrol FSM

Write a finite state machine that drives the robot forward until it sees a wall closer than 100 mm, turns right 90°, then continues. The robot should patrol indefinitely. The whole routine should be a clean FSM with named states, breaks, and timeouts.

States to include: DRIVE_FORWARD, TURNING, STOPPED.
Print the current state to the Brain screen so you can watch the FSM in action.
Add an exit condition (e.g. button on the controller) that moves the FSM to STOPPED.

No solution is given for this challenge.

Find the Bug

Two state-machine programs with intentional errors. Spot the bug.

Problem 11A

This FSM is supposed to drive to the ring, then grab it, then drive to the goal. Instead, the moment DRIVE_TO_RING finishes, the robot does GRAB_RING, DRIVE_TO_GOAL, and SCORE all in the same loop iteration. Why?

switch (state) {
  case DRIVE_TO_RING:
    if (DistanceSensor.objectDistance(mm) < 200) {
      state = GRAB_RING;
    }
  case GRAB_RING:
    Intake.spin(forward);
    wait(800, msec);
    state = DRIVE_TO_GOAL;
  case DRIVE_TO_GOAL:
    // ...
    state = SCORE;
  case SCORE:
    // ...
    state = DONE;
}

Problem 11B

This FSM enters DRIVE_TO_RING and stays there forever, even when the ring is right in front of the robot. The print statement shows state = 0 the whole time. What's missing?

AutoState state = DRIVE_TO_RING;

while (state != DONE) {
  switch (state) {
    case DRIVE_TO_RING:
      LeftMotor.spin(forward, 50, percent);
      RightMotor.spin(forward, 50, percent);
      if (DistanceSensor.objectDistance(mm) < 200) {
        LeftMotor.stop();
        RightMotor.stop();
        // (missing line here)
      }
      break;
    // ... other cases ...
  }
  wait(20, msec);
}

← Week 10 Next: Outside the Box →