State machine examples

State machine example with helper functions to set motor directions and set LED states.

int state = 1;

void setup()
{
  // Set up pins etc.
}

void loop()
{
  int fls, frs, bls, brs;
  
  // Read all sensors
  fls = analogRead(0);
  frs = analogRead(1);
  bls = analogRead(2);
  brs = analogRead(3);

  // Implement the actions for the current state
  if (state == 1)
  {
    // State 1 actions
    setMotors(1, 1); // both motors forwards
    setLEDs(state);
    
    // State transition?
    if (frs > 512) state = 2;
    if (fls > 512) state = 3;
  }
  else if (state == 2) // frs hits white first
  {
    // State 2 actions
    setMotors(1, 0); // LM forward, RM stop
    setLEDs(state);

    // State transition?
    if (fls > 512) state = 4;
  }
  else if (state == 3) // fls hits white first
  {
    // State 3 actions
    setMotors(0, 1); // LM stop, RM forward
    setLEDs(state);

    // State transition?
    if (frs > 512) state = 4;
  }
  else if (state == 4)
  {
    // State 4 actions
    setMotors(-1, -1); // both motors reverse
    setLEDs(state);

    // State transition?
    if (bls > 512 || brs > 512) state = 1;
  }
}

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(int current_state)
{
  digitalWrite(6, current_state == 1);
  digitalWrite(7, current_state == 2);
  digitalWrite(8, current_state == 3);
  digitalWrite(9, current_state == 4);
}

State machine example with state timer and function to change state.

int state;
unsigned long stateEnteredTime;

void setup()
{
  // Set up pins etc

  // Begin in state 1
  setState(1);
}

void loop()
{
  int fls, frs, bls, brs;
  float d;
  
  // Read all sensors
  fls = analogRead(0);
  frs = analogRead(1);
  bls = analogRead(2);
  brs = analogRead(3);
  d = getDistance();

  // Implement the actions for the current state
  if (state == 1) // spin / search
  {
    // State 1 actions
    setMotors(1, -1); // LM forward, RM reverse
    setLEDs(state);
    
    // State transition?
    if (d < 0.7) setState(2);
  }
  else if (state == 2) // pause
  {
    // State 2 actions
    setMotors(0, 0); // LM stop, RM stop
    setLEDs(state);

    // State transition?
    // 1s elapsed -> state 3
    if (millis() - stateEnteredTime > 1000) setState(3);
    if (d > 0.7) setState(1);
  }
  else if (state == 3) // attack
  {
    // State 3 actions
    setMotors(1, 1); // LM forward, RM forward
    setLEDs(state);

    // State transition?
    if (d > 0.7) setState(1);
    if (fls > 512) setState(4);
    if (frs > 512) setState(5);
  }
  else if (state == 4) // straighten up right
  {
    // State 4 actions
    setMotors(0, 1); // LM stop, RM forward
    setLEDs(state);

    // State transition?
    if (frs > 512) setState(6);
  }
  else if (state == 5) // straighten up left
  {
    // State 5 actions
    setMotors(1, 0); // LM forward, RM stop
    setLEDs(state);

    // State transition?
    if (fls > 512) setState(6);
  }
  else if (state == 6) // reverse
  {
    // State 6 actions
    setMotors(-1, -1); // LM reverse, RM reverse
    setLEDs(state);

    // State transition?
    // 2s elapsed -> state 1
    if (millis() - stateEnteredTime > 2000) setState(1);
    if (d < 0.7) setState(3);
  }
}

void setState(int s)
{
  state = s;
  stateEnteredTime = millis();
}

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);
}