Program Your Robot with a State Machine (Arduino Example)

Most robot programs start simple: drive forward, and if something is close, back up and turn. Then you add a stop button, a "stuck" check, a line sensor… and the code turns into a tangle of nested ifs and delay()s where the robot ignores the button for two seconds while it's turning. The cure is a pattern used in almost every real robot and machine: the finite state machine.

The idea

At any moment the robot is in exactly one state, such as driving, backing up, turning or stopped. Each state has:

• What to do while in it (set the motors).
• When to leave and which state to go to next (a sensor reading, a timer, a button).

Draw this diagram on paper before you write code. If you can't draw it, the code will be confusing too.

The building blocks

1. An enum lists the states with readable names:

enum State { DRIVING, BACKING_UP, TURNING, STOPPED };
State state = DRIVING;

2. A switch runs the code for the current state, once per pass through loop().

3. A timestamp records when the current state started, so timed states use millis() instead of delay():

unsigned long stateStart = 0;

void changeState(State next) {
  state = next;
  stateStart = millis();
}

Because no state waits with delay(), loop() keeps running many times per second and the robot reacts to the stop button within a few tens of milliseconds, whatever it's doing.

Complete example: obstacle-avoiding robot

Hardware: an HC-SR04 ultrasonic sensor, an L298N or TB6612 motor driver with two DC motors, and a push button to GND. See how ultrasonic sensors work and why you need a motor driver for the wiring.

enum State { DRIVING, BACKING_UP, TURNING, STOPPED };
State state = STOPPED;                 // start safely, press the button to go
unsigned long stateStart = 0;
int turnDir = 1;                       // +1 right, -1 left

// Pins
const int TRIG = 7, ECHO = 8;
const int ENA = 5, IN1 = 2, IN2 = 3;   // left motor
const int ENB = 6, IN3 = 4, IN4 = 9;   // right motor
const int BUTTON = 12;

// Tuning
const int OBSTACLE_CM = 20;
const unsigned long BACK_MS = 400;
const unsigned long TURN_MS = 350;

void setMotors(int left, int right) {  // -255 .. 255
  digitalWrite(IN1, left > 0);
  digitalWrite(IN2, left < 0);
  analogWrite(ENA, abs(left));
  digitalWrite(IN3, right > 0);
  digitalWrite(IN4, right < 0);
  analogWrite(ENB, abs(right));
}

long readDistanceCm() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  unsigned long us = pulseIn(ECHO, HIGH, 25000UL);  // max ~25 ms
  if (us == 0) return 999;                           // no echo = nothing close
  return us / 58;
}

const char* stateName(State s) {
  switch (s) {
    case DRIVING:    return "DRIVING";
    case BACKING_UP: return "BACKING_UP";
    case TURNING:    return "TURNING";
    default:         return "STOPPED";
  }
}

void changeState(State next) {
  if (next == TURNING) turnDir = random(2) ? 1 : -1;   // pick a side
  state = next;
  stateStart = millis();
  Serial.println(stateName(next));
}

void setup() {
  Serial.begin(9600);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  pinMode(BUTTON, INPUT_PULLUP);
  randomSeed(analogRead(A0));
  changeState(STOPPED);
}

long distanceCm = 999;               // latest reading, shared by all states

void loop() {
  // Events that apply in every state: the button toggles STOPPED.
  // The button must stay in its new position for 30 ms to count,
  // so contact bounce on press or release can't toggle twice.
  static bool stableBtn = HIGH, lastRaw = HIGH;
  static unsigned long lastEdge = 0;
  bool raw = digitalRead(BUTTON);
  if (raw != lastRaw) { lastEdge = millis(); lastRaw = raw; }
  if (millis() - lastEdge > 30 && raw != stableBtn) {
    stableBtn = raw;
    if (stableBtn == LOW) changeState(state == STOPPED ? DRIVING : STOPPED);
  }

  // Ping the ultrasonic sensor every 60 ms, not on every pass
  static unsigned long lastPing = 0;
  if (millis() - lastPing >= 60) {
    lastPing = millis();
    distanceCm = readDistanceCm();
  }

  unsigned long inState = millis() - stateStart;

  switch (state) {
    case DRIVING:
      setMotors(180, 180);
      if (distanceCm < OBSTACLE_CM) changeState(BACKING_UP);
      break;

    case BACKING_UP:
      setMotors(-150, -150);
      if (inState >= BACK_MS) changeState(TURNING);
      break;

    case TURNING:
      setMotors(150 * turnDir, -150 * turnDir);
      if (inState >= TURN_MS) changeState(DRIVING);
      break;

    case STOPPED:
      setMotors(0, 0);
      break;
  }
}

The sensor is read on its own 60 ms timer (the HC-SR04 needs that gap so old echoes don't cause false readings), and each reading can take up to 25 ms, so the loop still checks the button dozens of times per second. The Serial.println() in changeState() prints every transition, which makes debugging easy: the Serial Monitor shows exactly what the robot "thinks". Turn times depend on your motors and battery; tune TURN_MS until a turn is about 90°.

Why this structure scales

Adding a behavior means adding a state and its arrows, without touching the others:

• STUCK: if the robot has been in DRIVING for 10 seconds without the distance changing, back up longer.
• FOLLOW_LINE: switch to line following when the sensors see a line (see how a line-following robot works).
• LOW_BATTERY: from any state, go to a slow "return home" mode.

Compare this with the original obstacle-avoiding robot code: the behavior is the same, but now every rule has a clear place.

Tips for clean state machines

1. Set outputs at the start of each state, not scattered around the code.
2. Change states only through one function (changeState()), so timestamps and logging never get forgotten.
3. Never use delay() inside a state. Use the time in the state instead.
4. Handle "global" events (emergency stop, low battery) before the switch, as the button is here.
5. Start in a safe state (STOPPED), so the robot doesn't drive off the table on power-up.

Beyond robots

The same pattern runs traffic lights, vending machines, washing machines and game logic. Any time your code has "modes", a state machine will make it simpler.