|
[DPRG] Robot Code: subsumption behavior examples
Subject: [DPRG] Robot Code: subsumption behavior examples
From: David P. Anderson
dpa at io.isem.smu.edu
Date: Mon Mar 19 14:53:04 CDT 2007
Howdy,
Here are a couple of example behaviors based on last week's blow-by-blow
description of a sample subsumption cycle at:
<http://list.dprg.org/archive/2007-March/030426.html>
Those comments are also included here, just pasted from the previous
posting. Included are two example implementations for behavior tasks
in a multitasking environment, like the Flynn/Jones examples in
"Mobile Robots," and also the same behavior implemented as an AFSM for
inclusion in a large sensor loop as used SR04 and jBot, as described
in the the previous posts.
These are coding examples and not the actual code running on those robots,
which are somewhat more complex, but the principles remain the same.
/* ---------------------------------------------------------------------- */
Some practical subsumption behavior examples,
And a cyclic description of their execution.
Here are two simple examples, an IR avoidance behavior and a
bumper behavior, one "servo" and one "ballistic" behavior. Assume
a subsumption loop running at 20Hz for this example.
A. An IR avoidance behavior.
This is simplified code for an IR avoidance behavior, hopefully to make
it easier to follow along with the cyclic description:
int ir_task()
{
extern LAYER ir; /* C struct for ir_behavior output */
int detect = read_ir_sensors(); /* read the IR sensors detection status */
if (detect == LEFT) { /* IR reflection detected to the left */
ir.cmd = HALF_SPEED; /* request robot to slow down */
ir.arg = RIGHT_TURN; /* and turn to the right */
ir.flag = TRUE; /* the arbitrater */
} else {
if (detect == RIGHT) { /* IR reflection detected on the right */
ir.cmd = HALF_SPEED; /* request the robot to slow down */
ir.arg = LEFT_TURN; /* and turn to the left */
ir.flag = TRUE; /* signal the arbitrater */
} else {
if (detect == BOTH) { /* IR reflection directly ahead */
ir.cmd = ZERO_SPEED; /* request robot to slow down to 0 */
ir.arg = keep_turning(); /* and turn same direction as last */
ir.flag = TRUE; /* signal arbitrater */
} else {
ir.flag = FALSE; /* else turn off subsumption flag */
}}}
}
Using the ir code as a standalone task, as in the Flynn/Jones example
from "Mobile Robots" requires only that the function is called inside
of a loop with the approriate delay, like this:
void ir_behavior()
{
int t = sysclock;
while (1)
ir_task()
t = tsleep(&t,50);
end
}
Most subsumption behaviors take on one of the two above forms.
/* ---------------------------------------------------------------------- */
B. IR avoiding an obstacle on the right: a cyclic description.
The IR avoidance detector "sees" a detection on the right, so it
outputs a command to turn left and sets its subsumption flag
to active, to signal the arbitrater. Assume it is the highest
priority flag for that 1/20th of a second, so the arbitrater sends
its command to the motors, and that is what the motors do
(ignoring any PID slewing,etc) for the next 50 millisecs, until
the next time through the subsumption loop. But that's
all, just for the next 50 milliseconds.
Now, 1/20 second later as the loop executes again, the robot has
hardly moved at all, and the IR avoidance detector still "sees" the
detection on the right, and again outputs a command to turn left,
and again sets its subsumption flag to active to signal the
arbitrater. Again it is the highest priority layer signaling and
the arbitrater again passes its command to the motors to turn
left, and that's what they do for the next 50 milliseconds. But
only for the next 50 milliseconds.
This processes continues each time through the subsumption
loop, with the IR avoidance winning the priority contest in little,
50 millisecond chunks, and passing its command to turn left
to the motors each time 20 times a second. After, lets say,
2 seconds (40 times through the loop, 40 turn left commands
to the motors) the robot has finally turned left far enough that
the next time through loop, the IR avoidance detector no longer
"sees" a detection on the right. So on that pass through the
loop, the IR avoidance behavior's flag becomes FALSE (no
detection) and some other lower priority behavior gets to pass
its commands to the motors.
The output from the IR avoidance behavior goes away as soon
as the detection goes away, (or within 50 ms thereof).
/* ---------------------------------------------------------------------- */
C. A Collision recovery, ballistic behavior.
Here is simplified code for a bumper behavior, to make it
easier to follow along. This behavior is illustrated
two ways; first as a multi-tasking behavior, and then
as an Augmented Finite State Machine (AFSM).
void bumper_behavior() /* Collision recovery as concurrent task */
{
extern LAYER bump; /* C structy for bumper_behavior output */
int delay, bumper; /* locals */
while (1) { /* endless loop */
bumper = read_bumper_switches(); /* read bumper status */
if (bumper) { /* if we got a bumper press */
bump.cmd = BACKUP_HALFSPEED; /* request reverse speed */
bump.arg = 0; /* straight back */
bump.flag = TRUE; /* signal the arbitrater */
msleep(1000); /* and back up for 1 second */
bump.cmd = HALF_SPEED; /* then request forward speed */
if (bump == LEFT) /* and turn away from bump */
bump.arg = RIGHT_TURN;
else bump.arg = LEFT_TURN;
msleep(500); /* for 1/2 second */
bump.flag = FALSE; /* behavior finished, reset flag */
} else {
bump.flag = FALSE; /* else no detections, reset flag */
msleep(10); /* and loop at 100HZ to be sure */
/* we don't miss any bumps */
}
}
}
Here is the same collision recovery behavior coded as an Augmented
Finite State Machine, suitable for inclusion in a sensor() loop
as illustrated for SR04 and jBot above.
/* persistent local variables are required for the AFSM */
int bumper, /* bumper switches */
bumper_state, /* AFSM variable */
bumper_timer; /* ballistic behavior timer */
int bumper_task() /* Collision recovery behavior as AFSM */
{
extern LAYER bumper;
if (timer_done(bumper_timer)) { /* just exit if not timed out */
/* else test behavior state */
if (bumper_state == 0) { /* state 0 == looking for bumps */
bumper = read_bumper_switches; /* so read the switches */
if (bumper) { /* bump detected, so backup */
bumper.cmd = BACKUP;
bumper.arg = 0;
bumper.flag = TRUE;
bumper_timer = sysclock + 1000;
bumper_state = 1; /* state 1 == backing up */
} else {
bumper.flag = FALSE; /* no detections, so exit */
}
} else {
if (bumper_state == 1) { /* state 1 completed */
bumper.cmd = HALF_SPEED; /* forward at half speed */
if (bumper == LEFT) /* turn away from bump */
bumper.arg = RIGHT_TURN;
else bumper.arg = LEFT_TURN;
bumper_timer = sysclock + 500; /* for 1/2 second */
bumper_state = 2; /* state 2 == turning away */
} else {
bumper.flag = FALSE; /* state 2 complete, behavior end */
bumper_state = 0; /* reset flag and bumper state */
}
}
}
/* ---------------------------------------------------------------------- */
D. Recovery from collision: a cyclic description.
Now the second example, a ballistic bumper behavior. The
bumper gets a press on the right, and triggers the start of
a ballistic behavior. The first state outputs a command to
backup and sets its subsumption flag to active, to signal the
arbitrater, and also sets an internal TIMER to, lets say one
second. Assume it is the highest priority layer asserting
its flag, and the backup command is passed by the arbitrater
to the motors. So for the next 50 milliseconds, that's what
the motors do.
Now, 1/20 second later as the loop executes again, but the
layer does _not_ test the trigger condition again, as in the
case of the IR avoidance behavior, but rather tests the
TIMER to see if it has timed out. That is why it is a
ballistic behavior. Its termination depends on an internal
timer, rather than the absence of an external trigger. When
it tests the timer it sees that it has not timed out, so the layer
leaves the output command = backup, and leaves the
subsumption flag = active. And for the next 50
milliseconds, the robot continues to backup.
This continues for another 18 times though the subsumption
loop. On the 21st time through the loop, the timer has
expired, and the layer sequences to the next state, which
is a turn left command. It leaves the subsumption flag
as active, and resets the internal TIMER to, say, half a
second, and sets its output command = turn left.
And it does that for the next 10 times through the loop,
each time testing the TIMER, not the trigger condition.
Finally, the TIMER times out, the ballistic behavior is
finished, and its subsumption flag is set to FALSE.
At that time, any lower priority task that has its flag = TRUE
will regain control of the robot.
18 March 2007
Dallas, Tx,
dpa
More information about the DPRG mailing list
|
