An Introduction and Example of Finite State Machines

Finite State Machines (FSM) are often an excellent design pattern for robot control software. As I was developing my first robotic vehicle, I started with just modifying and expanding upon some sample code I found online.  This worked for a robot moving around at random, avoiding obstacles.  But once I wanted to add positioning and navigation, it became difficult to keep track of where the logic should be at each step and each time the main control loop executed.  I realized that a state machine approach was the solution, and it’s worked out beautifully.

What is a Finite State Machine?  You can find the formal definition here at Wikipedia, but as David Stonier-Gibson puts it in his tutorial:

The formal, mathematical definition of an FSM is such brain numbing, eye popping mumbo jumbo I feel certain that 9 out of 10 electronic engineering and IT students switch off in the first 5 minutes of the FSM lecture series, never to ever benefit from the power of FSMs in their work. This is not difficult stuff, it’s just made to look difficult by the academics!

The basic concept of an FSM is that the system is, at any time, in a specific, pre-defined state.  Multiple states are defined, along with the internal or external events that cause the system to transition from one state to another.  The functions that need to be performed for each transition from one state to another are then determined, and, of course, you have functions to be performed when in a given state.  Not all events trigger a state transition, some are processed and handled with the system staying in the same state.  It’s easiest to explain by example.  A good, short tutorial with examples can be found in the online article Application Design Patterns: State Machines, along with some useful design hints.  Below, I use the state-machine model I developed for my MARV-1 autonomous dead-reckoning vehicle as an example.

State Machines can be represented in several ways.  One approach is a state transition matrix.  Below is the matrix for the code for my MARV-1 autonomous vehicle:

State Transition Matrix

The topmost row shows the seven states that the robot can be in.  The leftmost column shows the internal and external events that can cause a change from one state to another.  For example, “Button 1 pressed” is an external event.  If MARV-1 is currently in the Off state, it transitions to the Orienting state.  If it’s in any other state, it transitions to Off.  “Completed calculating next heading” would be an internal event that causes a transition from the Orienting state to the Turning state.  If, instead, the last waypoint in the list had already been processed, then there would be a transition from the Orienting to the Off state.

In C/C++/Arduino code, states are typically coded using the Switch command.  Each Case ends with a break;, since the system can only be in one state at a time.  There can be code to run each time a state is entered for the first time and code to run when you exit a particular state.  This code can be dependent on the specific transition, e.g., the exit code from Orienting to Off is different than from Orienting to Turning.  I found it easier to use exit code rather than entrance code wherever possible, because then I didn’t have to set a flag and check each time through the main loop to determined if I was entering a state for the first time or not.

In fact, 95 percent of the code in the main Loop for my MARV-1 vehicle is inside a single Switch statement.  The exceptions are checking for the Button 1 press, since that must be done in every state, and updating position and orientation, since, with the exception of the Off state, that must always be done.

You’ll notice that most of the cells in the state transition matrix are empty.  That’s because most events only affect one or two states.  That means that you don’t need to check for those events when in any of the other states.   The state machine design pattern makes coding simpler and much, much easier to follow.  It also  allows you to change out the details within a state (inside a specific case in your code) without affecting the rest of your code.  The exception to this is the transition triggers themselves and the exit processing code.

Another way of designing or documenting your Finite State Machine is with a state machine diagram.  Here’s the same information for MARV-1, presented in pictorial form:

State Machine Diagram

In the case of MARV-1, I started my planning figuring on an Off state and an On state.  Of course, the On state quickly was subdivided into more.  Since MARV-1 uses tank-style turns and always either moves in a straight line or turns in place, it was logical to have a Turning state and a Traveling state.  As I worked on the code, I found it made sense to break out the Orienting from the Turning, even though the Orienting is just some calculations that complete in a single pass through the main Loop.  Similarly, it became easier to code to break the obstacle avoidance maneuvering into 3 states, one for each step in the 3 step obstacle avoidance approach I decided on:  Back up, turn right, go forward a ways, hopefully clearing the original obstacle.  As you can see from the table or picture, if MARV-1 is still pointing towards an obstacle after turning, it goes back to the Avoid:Backing state, as it does if it detects an obstacle while moving forward.

Finite State Machines are a simple and effective concept for robot software, and I encourage you to plan out your next project using this approach.

Some Debugging Frustration

My first robot, MARV-1 (Mike’s Autonomous Robotic Vehicle #1), is doing fine at avoiding obstacles, but it gets easily confused and sometimes gets literally spinning in circles.

I need to try to trace the cause of the endless spinning. My hypothesis is that I have the tolerance parameter for turns set too tight, so with the lack of precision in the heading, it can get in a mode where it’s never getting close enough to the desired heading to stop turning.

UPDATE: My hypothesis was incorrect. The problem was that while I normalize the robot’s heading to always be between -Pi and +Pi radians, I wasn’t normalizing the new target heading of (heading – Pi/4 radians) to turn away from the obstacle. So if that every was less than -Pi, the heading could never be close to the target heading, and the robot kept spinning.

I may also try to modify the marking on the gears so that I get a pulse every 1/8th of a wheel rotation rather than every 1/4.

UPDATE: This worked, and does seem to improve navigation, but errors still build up quickly on the heading, and hence position, as they tend to do with only dead reckoning for navigation.

I also need to modify the obstacle avoidance routine. Right now, when MARV-1 detects an obstacle it backs up, rotates right about 45 degrees, and then tries to move forward about 2 feet before recalculating a new heading and distance to the next waypoint. When MARV-1 is pointed to the right of the waypoint, this results in it turning away even further. I think I’ll try to have it turn in whatever direction is closer to the heading to the next waypoint.

Move now, don’t delay(); the metro library

The classic “hello world” of the Arduino world includes something like:

void loop()
{
  digitalWrite(ledPin, HIGH);   // sets the LED on
  delay(1000);                  // waits for a second
  digitalWrite(ledPin, LOW);    // sets the LED off
  delay(1000);                  // waits for a second
}

Simple sample code for robotic vehicles may include something similar, such as

//Move Forward for 10 seconds
advance (255,255);
delay(10000);

where the advance function is previously defined. This approach, using delay(), works fine until you want your robot to do something else while moving, like check sensors for obstacles, or see if a button was pushed to turn it off, or updating its position based on sensor input. The problem of course, is that while the delay is running, the cpu does nothing (although it will process interrupts). So clearly using delay() for this purpose is not a good idea.

Rather than having your code sit idle at a point in the loop function, spinning its figurative wheels, you want the software to continue looping through the main loop, but stopping when a set time is reached. One approach is to use a timer interrupt. Have your code continue through the loop, but set an interrupt service routine that sets a flag, signifying the time has elapsed and it’s time to stop doing what you were doing (e.g., moving forward) and do something else. An introduction to timers and interrupts can be found on the Let’s Make Robots site, and you should also check the Arudino reference page on interrupts .  If you’d like to keep things simpler, there are Arduino libraries for using timer interrupts as well.

An alternative approach is to use the metro library.  The metro library provides an alternative to using timer interrupts.  Because it does not use interrupts, the code in your loop will need to check to see if the timer has expired or not.  The sample code below works on my robot that uses an Arduino-compatible Romeo controller and RP5 chassis.  It was based on the example provided by DFRobot but adds a) using switch 1 on the Romeo board as a start/stop switch and b) instead of using delay(), it uses the Metro library.

The key section of the code is the nextMovement function. It’s called every time the main loop executes. A timer called movementMetro was already initialized as part of the setup. This nextMovement function checks if the timer has gone off, and if it has, changes the movement case to the next in line and resets the value for the appropriate time. One tricky part is that the timer value is for the NEXT movement, not the one in the movement just completed.

void nextMovement (int next,unsigned long wait) // Set case and delay for next movement
  {
    if (movementMetro.check() == 1) {
      movement = next;
      movementMetro.interval(wait);
    }
  }

Here’s the full, working code. When run, you need to push button 1 once the code is loaded. The robot then moves forward, turns, move forward, reverses, turns, and reverses again, with pauses between each movement. You can stop and restart the cycle by using switch 1.

#include <Metro.h>

// Setup movement timer using the Metro library
Metro movementMetro = Metro(4000);

// Buttons vars

const int key_S1_5 = 7;

int state = false; // Off if 0, On if 1

int buttons_check(){
 // This function checks button 1 and returns
 // 0 if no button pressed, 1 if pressed
 // Priorities: S1, S2, S3, S4, S5
 int w = analogRead(key_S1_5);

#define vS2 130
 if ( w < vS2/2 ){
  return 1;
}
 return 0;
}//End buttons_check()

 // Set up for running motors
int E1 = 5;  // digital output pin sending PWM power to the motor
int E2 = 6;  // digital output pin sending PWM power to the motor
int M1 = 4;  // controls is motor moves forward or backwards
int M2 = 7;  // controls is motor moves forward or backwards

void forward(byte a,byte b)		//Move forward
        {
          analogWrite(E1,a);
          digitalWrite(M1,HIGH);
          analogWrite(E2,b);
          digitalWrite(M2,HIGH);
        }

void stop(void)                    	//Stop
        {
          digitalWrite(E1,LOW);
          digitalWrite(E2,LOW);
        }

void backward (byte a,byte b)		//Move backward
        {
          analogWrite (E1,a);
          digitalWrite(M1,LOW);
          analogWrite (E2,b);
          digitalWrite(M2,LOW);
}

void Left (byte a,byte b)		//Turn left
        {
          analogWrite (E1,a);
          digitalWrite(M1,LOW);
          analogWrite (E2,b);
          digitalWrite(M2,HIGH);
        }
void Right (byte a,byte b)		//Turn right
        {
          analogWrite (E1,a);
          digitalWrite(M1,HIGH);
          analogWrite (E2,b);
          digitalWrite(M2,LOW);
        }
int movement = 1; // movement controls which movement (forward, turn, etc.)

void nextMovement (int next,unsigned long wait) // Set case and delay for next movement
  {
    if (movementMetro.check() == 1) {
      movement = next;
      movementMetro.interval(wait);
    }
  }

void setup(){
    for(int i=4;i<=7;i++)
    pinMode(i, OUTPUT);
    Serial.begin(9600);
}// End setup
void loop(){

// This section checks button 1, which functions as stop/start switch, initially stopped
 int button = buttons_check();
 if ( button == 1 ) {
 // Button 1 has been pressed
  state = !state;
 }
 Serial.println(state);

 // Whenstopped after running, reset to original start mode
 if (state == false) {
   stop(); //Stop
   movement = 1;
   movementMetro.interval(4000);
 }
 else {
   Serial.println(movement);

 // Each case sets movement command, signals next movement, and resets timer for duration OF NEXT MOVEMENT
   switch (movement) {
     case 1:
       forward(255,255); //Forward at full speed
       nextMovement(2, 5000);
       break;
     case 2:
       stop(); //Stop
       nextMovement(3, 650);
       break;
     case 3:
       Right(255,255);  //Right at full speed
       nextMovement(4, 5000);
       break;
     case 4:
       stop(); //Stop
       nextMovement(5, 4000);
       break;
     case 5:
       forward(255,255); //Forward at full speed
       nextMovement(6, 5000);
       break;
     case 6:
       stop(); //Stop
       nextMovement(7, 4000);
       break;
     case 7:
       backward(255,255); //backward at full speed
       nextMovement(8, 5000);
       break;
     case 8:
       stop(); //Stop
       nextMovement(9, 650);
       break;
     case 9:
       Left(255,255);  //Left at full speed
       nextMovement(10, 5000);
       break;
     case 10:
       stop(); //Stop
       nextMovement(11, 4000);
       break;
     case 11:
       backward(255,255); //backward at full speed
       nextMovement(12, 5000);
       break;
     case 12:
       stop(); //Stop
       nextMovement(1, 4000);
       break;
   }
 }
}// End loop

Nothing fancy, but it was my start on getting my project underway and moving beyond using delay().