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 = Stopped2 = Forward3 = Avoiding Obstacle4 = Turn Left5 = Turn Right6 = ReverseThe 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 bodysetup(){// There is nothing in setup related to SMLib}// Typical events to manage an Autonomous Robot in the main loopvoid loop() {get_sensors(); // Read all sensors, like proximity, etc...EXEC(m1); // Execute the State Machineprovide_feedback(); // Send status updates via Serialget_serial(); // Grab commands from Serial}/**************************************************************************/
This is the FSM.ino module:
/**********************************************************************/State machine m1 is for motion controlthe states are as follows:1 = Stopped2 = Forward3 = Avoiding Obstacle4 = Turn Left5 = Turn Right6 = ReverseMotionStop dictates how much time is spend in any state.*/State m1s1h(){ // m1s1 is --- Motion:StoppedSerial.println("Motion:Stopped (State 1)");halt();}//m1s1h()State m1s1b(){if(m1.Timeout(5000)){ // Maximum 5 seconds idle timem1.Set(m1s2h, m1s2b);Serial.println("changing to Motion:Forward (State 2)");};}//m1s1b()State m1s2h(){ // m1s1 is --- Moving ForwardSerial.println("Motion: Forward (State 2)");}//m1s2h()State m1s2b(){if(m1.Timeout(MotionStop)){ // Maximum 5 seconds forward motionm1.Set(m1s1h, m1s1b);Serial.println("changing back to Motion:Stopped (State 1)");}if(Distance > MINDIST){BlinkM_fadeToRGB( blinkm_addr, 0,255,0 ); // Display GREEN Statusforward();} else {Serial.println("changing to Motion:Avoid Obstacle (State 3)");m1.Set(m1s3h, m1s3b);}}//m1s2b()State m1s3h(){ // m1s3 is --- Avoid ObstacleSerial.println("Motion: Avoid Obstacle (State 3)");MotionStop = 200; // Set turn time for Obstacle AvoidanceBlinkM_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 LeftSerial.println("Motion: Left Turn (State 4)");}//m1s4h()State m1s4b(){if(m1.Timeout(MotionStop)){ // Maximum 5 seconds forward motionm1.Set(m1s1h, m1s1b);Serial.println("changing back to Motion:Stopped (State 1)");}BlinkM_fadeToRGB( blinkm_addr, 0,255,0 ); // Display GREEN Statusleft();}//m1s4b()State m1s5h(){ // m1s1 is --- Turning RightSerial.println("Motion: Right Turn (State 5)");}//m1s5h()State m1s5b(){if(m1.Timeout(MotionStop)){ // Maximum 5 seconds forward motionm1.Set(m1s1h, m1s1b);Serial.println("changing back to Motion:Stopped (State 1)");}BlinkM_fadeToRGB( blinkm_addr, 0,255,0 ); // Display GREEN Statusright();}//m1s5b()State m1s6h(){ // m1s1 is --- ReverseSerial.println("Motion: Reversing (State 6)");}//m1s6h()State m1s6b(){if(m1.Timeout(MotionStop)){ // Maximum 5 seconds forward motionm1.Set(m1s1h, m1s1b);Serial.println("changing back to Motion:Stopped (State 1)");}BlinkM_fadeToRGB( blinkm_addr, 0,255,0 ); // Display GREEN Statusreverse();}//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
