Introducing an Arduino Finite State Machine library to PenguinBot

First off... PenguinBot was featured on Hackaday last Friday!  Yay!

  PenguinBot Follows Light, Goes Screech in the Night

I spent a little time yesterday reworking my code for PenguinBot to replace the homemade state machine of  If/then/else and switch statements, with a proper State Machine Library.

I specifically chose the SMLib  State Machine Library because of it's simplicity and lack of overhead.   I know there are other full featured State Machine Libraries out there, but this one serves the purpose well. 

After all..  it's just a penguin that avoids obstacles...  most of the time... more or less... 

This library allows you to have a Head and Body for each state.  

The "Head" is for initializing variables each time you transition into the state.  The "Body" contains the actions that are to take place each time you loop through the state,such as incrementing a counter, or checking a sensor.

Regardless of whether you are in "Manual Mode", "Object Avoidance Mode", or the yet to be implemented "Light Following Mode",  State Machine "m1" controls motion. 

The states are as follows:

1 = Stopped

2 = Forward

3 = Avoiding Obstacle

4 = Turn Left

5 = Turn Right

6 = Reverse

The Global Variable "MotionStop" dictates how much time is spend in any state.

Current (imperfect) code here:

Setup and Initialization of the State Machine :

/************************************************************************/

#include <SM.h>

SM m1(m1s1h, m1s1b);  //Initialize state machine m1 with head and body

setup(){

// There is nothing in setup related to SMLib

}

//  Typical events to manage an Autonomous Robot in the main loop

void loop() {
  get_sensors();             // Read all sensors, like proximity, etc...

  EXEC(m1);                  // Execute the State Machine

  provide_feedback();        // Send status updates via Serial
  get_serial();              // Grab commands from Serial 

}

/**************************************************************************/

This is the FSM.ino module:

/**********************************************************************/

State machine m1 is for motion control

the states are as follows:

1 = Stopped

2 = Forward

3 = Avoiding Obstacle

4 = Turn Left

5 = Turn Right

6 = Reverse

MotionStop dictates how much time is spend in any state.

*/

State m1s1h(){ // m1s1 is --- Motion:Stopped

  Serial.println("Motion:Stopped (State 1)");

  halt();

}//m1s1h()

State m1s1b(){

  if(m1.Timeout(5000)){ // Maximum 5 seconds idle time

    m1.Set(m1s2h, m1s2b);

    Serial.println("changing to Motion:Forward (State 2)");

  };

}//m1s1b()

State m1s2h(){ // m1s1 is --- Moving Forward

  Serial.println("Motion: Forward (State 2)");

}//m1s2h()

State m1s2b(){

  if(m1.Timeout(MotionStop)){ // Maximum 5 seconds forward motion

    m1.Set(m1s1h, m1s1b);

    Serial.println("changing back to Motion:Stopped (State 1)");

  }

  

  if(Distance > MINDIST){

   BlinkM_fadeToRGB( blinkm_addr, 0,255,0 ); // Display GREEN Status

   forward();

  } else {

    Serial.println("changing to Motion:Avoid Obstacle (State 3)");

    m1.Set(m1s3h, m1s3b);

  }

}//m1s2b()

State m1s3h(){ // m1s3 is --- Avoid Obstacle

  Serial.println("Motion: Avoid Obstacle (State 3)");

  MotionStop = 200; // Set turn time for Obstacle Avoidance

  BlinkM_fadeToRGB( blinkm_addr, 255,0,0 ); // Display Warning RED

}//m1s3h()

State m1s3b(){

  if(DistLeft > DistRight){

    m1.Set(m1s4h, m1s4b); // Change State to Left Turn

  } else if(DistRight > DistLeft){

    m1.Set(m1s5h, m1s5b); // Change State to Right Turn

  } else {

    Serial.println("changing back to Motion:Stopped (State 1)");

    m1.Set(m1s1h, m1s1b);

  }

}//m1s3b()

State m1s4h(){ // m1s1 is --- Turning Left

  Serial.println("Motion: Left Turn (State 4)");

}//m1s4h()

State m1s4b(){

  if(m1.Timeout(MotionStop)){ // Maximum 5 seconds forward motion

    m1.Set(m1s1h, m1s1b);

    Serial.println("changing back to Motion:Stopped (State 1)");

  }

   BlinkM_fadeToRGB( blinkm_addr, 0,255,0 ); // Display GREEN Status

   left();

}//m1s4b()

State m1s5h(){ // m1s1 is --- Turning Right

  Serial.println("Motion: Right Turn (State 5)");

}//m1s5h()

State m1s5b(){

  if(m1.Timeout(MotionStop)){ // Maximum 5 seconds forward motion

    m1.Set(m1s1h, m1s1b);

    Serial.println("changing back to Motion:Stopped (State 1)");

  }

   BlinkM_fadeToRGB( blinkm_addr, 0,255,0 ); // Display GREEN Status

   right();

}//m1s5b()

State m1s6h(){ // m1s1 is --- Reverse

  Serial.println("Motion: Reversing (State 6)");

}//m1s6h()

State m1s6b(){

  if(m1.Timeout(MotionStop)){ // Maximum 5 seconds forward motion

    m1.Set(m1s1h, m1s1b);

    Serial.println("changing back to Motion:Stopped (State 1)");

  }

   BlinkM_fadeToRGB( blinkm_addr, 0,255,0 ); // Display GREEN Status

   reverse();

}//m1s6b()

/************************************************************************/

MotionStop duration is typically set either as a Serial Command parameter when in Manual Mode, or predetermined time lapse for "In Motion" or "Idle"  when in Autonomous Mode (Either Obstacle Avoidance or Light Following).

The exception to this, is in State 3 --- Avoid Obstacle.  In this state, it has been determined that an obstacle blocks the way ahead, and an assessment is done as to whether there is more room to the left or to the right.   "MotionStop" duration is set up to allow just enough time to turn the bot roughly 90 degrees. "MotionStop = 200; // Set turn time for Obstacle Avoidance"


Over the next few days, I will convert the rest of the code to use this library, and show the "Light Following Mode"



References:
Arduino-Pi: Of Finite State Machines and Robotics
https://github.com/michaeljball/PenguinBot

Arduino Playground: A novel and relaxed view on finite state machines
http://playground.arduino.cc/Code/FiniteStateMachine
Robot Virtual Worlds – Maze Crawler
Embedded Micro:  basic FSM to control a very simple robot.
http://www.mathertel.de/Arduino/FiniteStateMachine.aspx
http://hacking.majenko.co.uk/finite-state-machine