Discuss this article in the SERVO Magazine forums at http://forum.servomagazine.com
Figure 4
code isn’t that complex.
This is not to say that it is
easy to write! You will
spend a great deal of time
tweaking and tuning your
timing, and choosing your
state steps before your
behaviors work properly.
The FSM is a basic
cornerstone of embedded
programming, so if you are
inclined to get a job in
automation or embedded
engineering, put some time
in on these concepts —
you’ll need them!
Back to the
Hardware
While working with my
Roomba, I got tired of my
taped-on dead weights
falling off and my Roomba
stopping every time it
changed directions. So, I
Listing 3: DoEscape behavior.
break;
void DoEscape(void)
// Define a bumper response escape behavior
{
static uint32_t ticker = 0;
static uint8_t state = 0;
static uint8_t dir = 0;
if ((mBumpers == 0) && (state ==0))
// Don't do anything if nothing to do
{
return;
}
if (priority <= escape)
{
priority = escape;
switch(state)
{
case 0: // record bumper, backup
dir = mBumpers;
ticker = millis() + 1000; // 1 second
GoBackward();
GoSong();
state = 1;
Serial.print("Bumper dir: ");
Serial.println(dir,HEX);
break;
case 2: // choose turn
// direction
if (dir == RIGHT)
{
Serial.println("spin left");
SpinLeft();
}
else if (dir == LEFT)
{
Serial.println("spin right");
SpinRight();
}
else
{
Serial.println("ahead");
SpinRight();
}
ticker = millis() + 1000;
state = 3;
break;
case 1: // wait for backup
// done
if (ticker < millis())
{
state = 2;
Serial.println("Do turn");
}
case 3: // wait for turn done
if (ticker < millis())
{
state = 0;
dir = 0;
priority = idle;
Serial.println("All done");
}
break;
}
}
}
14 SERVO 04.2013