Example code: two state machines running in parallel

//
// State machine example with two parallel state machines.
// State machine A is responsible for motor control.
// State machine B is responsible for LED control.
//

int stateA, stateB;
unsigned long t0_A, t0_B;

void setup()
{
  // Set up pins etc

  // Begin both state machines in state 1
  setStateA(1);
  setStateB(1);

  // Open serial connnection to PC
  Serial.begin(9600);
}

void loop()
{
  int cs;
  
  // Read colour sensor
  cs = analogRead(0);

  // State machine A - motor control
  if (stateA == 1) // pivot forward and left
  {
    // State 1 actions
    setMotors(0, 1); // LM stop, RM forward
    
    // State transition?
    if (cs < 512) setStateA(2);
  }
  else if (stateA == 2) // pivot forward and right
  {
    // State 2 actions
    setMotors(1, 0); // LM forward, RM stop

    // State transition?
    if (cs > 512) setStateA(1);
  }

  // State machine B - LED control
  if (stateB == 1) // LED on
  {
    // State 1 actions
    setLEDs(1); // LED on
    
    // State transition?
    // 0.5s elapsed -> state 2
    if (millis() - t0_B > 500) setStateB(2);
  }
  else if (stateB == 2) // LED off
  {
    // State 2 actions
    setLEDs(0); // LED off

    // State transition?
    // 1.0s elapsed -> state 1
    if (millis() - t0_B > 1000) setStateB(1);
  }  
}

void setStateA(int s)
{
  stateA = s;
  t0_A = millis();

  Serial.print(t0_A);
  Serial.print(" stateA = ");
  Serial.println(stateA);
}

void setStateB(int s)
{
  stateB = s;
  t0_B = millis();

  Serial.print(t0_B);
  Serial.print("stateA = ");
  Serial.println(stateA);
}

void setMotors(int LM_direction, int RM_direction)
{
  digitalWrite(2, LM_direction > 0);
  digitalWrite(3, LM_direction < 0);
  digitalWrite(4, RM_direction > 0);
  digitalWrite(5, RM_direction < 0);
}

void setLEDs(unsigned int current_state)
{
  digitalWrite(6, current_state & 1);
  digitalWrite(7, current_state & 2);
  digitalWrite(8, current_state & 4);
  digitalWrite(9, current_state & 8);
}