Subsumption: An Example
If you have a knee-jerk reaction to this, you're on the right track.
This example code accompanies the paper Mobile Robot Control: The Subsumption Architecture and occam-pi and demonstrates a simple subsumptive architecture implemented in occam-pi.
#INCLUDE "player.inc"
#USE "course.lib"
-- Constants for motor commands
VAL INT motor.stop IS 0:
VAL INT motor.forward IS 1:
VAL INT motor.back.right IS 2:
PROC motor (CHAN INT motor.command?, CHAN MOTORS motor.control!)
VAL INT speed IS 50:
VAL INT turn.rate IS 40:
WHILE TRUE
INT command:
SEQ
motor.command ? command
CASE command
motor.stop
motor.control ! 0 ; 0 ; 0
motor.forward
motor.control ! speed ; 0 ; 0
motor.back.right
motor.control ! -speed ; 0 ; -turn.rate
ELSE
STOP
:
PROC motor.command.suppressor (VAL INT time, CHAN INT suppress?, in?, out!)
TIMER tim:
INITIAL INT timeout IS 0:
INITIAL BOOL suppressing IS FALSE:
INT value:
WHILE TRUE
PRI ALT
NOT suppressing & suppress ? value
SEQ
suppressing := TRUE
tim ? timeout
timeout := timeout PLUS time
out ! value
NOT suppressing & in ? value
out ! value
suppressing & tim? AFTER timeout
suppressing := FALSE
suppressing & suppress ? value
out ! value
:
PROC prevent.collision (CHAN INT minimum.distance?, CHAN INT motor.command!)
WHILE TRUE
INITIAL INT min IS 0:
SEQ
minimum.distance ? min
IF
min = 0
SKIP
min < 20
motor.command ! motor.stop
TRUE
motor.command ! motor.forward
:
PROC min.distance (SHARED CHAN LASER laser.data?, CHAN INT min.distance!)
LASER data:
WHILE TRUE
INITIAL INT min IS (MOSTPOS INT):
SEQ
CLAIM laser.data?
laser.data ? data
SEQ i = 0 FOR SIZE data
SEQ
IF
data[i] = 0
SKIP
data[i] < min
min := data[i]
TRUE
SKIP
min.distance ! min
:
PROC object.in.front (SHARED CHAN LASER laser.data?, CHAN BOOL object!)
LASER data:
WHILE TRUE
SEQ
CLAIM laser.data?
laser.data ? data
SEQ i = 75 FOR 90
IF
data[i] = 0
SKIP
data[i] < 150
object ! TRUE
TRUE
SKIP
:
PROC pivot.if.object (CHAN BOOL object?, CHAN INT motor.command!)
WHILE TRUE
BOOL flag:
SEQ
object ? flag
IF
flag
motor.command ! motor.back.right
TRUE
SKIP
:
PROC check.has.space (CHAN SONAR sonar.data?, CHAN BOOL no.space!)
WHILE TRUE
SONAR data:
SEQ
sonar.data ? data
SEQ i = 11 FOR 4
IF
data[i] < 50
no.space ! TRUE
TRUE
SKIP
:
PROC pivot.inhibitor (VAL INT time, CHAN BOOL inhibit, CHAN INT in?, out!)
TIMER tim:
INITIAL INT timeout IS 0:
INITIAL BOOL inhibiting IS FALSE:
INT data:
BOOL flag:
WHILE TRUE
PRI ALT
inhibit ? flag
SEQ
inhibiting := TRUE
tim ? timeout
timeout := timeout PLUS time
inhibiting & tim? AFTER timeout
inhibiting := FALSE
NOT inhibiting & in ? data
out ! data
inhibiting & in ? data
SKIP
:
PROC circle.or.line.follow (CHAN BYTE kyb?, scr!, err!)
CHAN MOTORS motor.control:
CHAN INT minimum.distance:
CHAN INT motor.command.in, motor.command.out, motor.command.suppress:
CHAN INT pivot.motor.command.in, pivot.motor.command.out:
SHARED ? CHAN LASER laser.data:
CHAN SONAR sonar.data:
CHAN BOOL object, inhibit:
PAR
motor(motor.command.out?, motor.control!)
brain.stem(motor.control?, laser.data!, sonar.data!, default.player.host, default.player.port)
min.distance(laser.data?, minimum.distance!)
prevent.collision(minimum.distance?, motor.command.in!)
object.in.front(laser.data?, object!)
pivot.if.object(object?, pivot.motor.command.in!)
motor.command.suppressor(10000, pivot.motor.command.out?, motor.command.in?, motor.command.out!)
pivot.inhibitor(100000, inhibit?, pivot.motor.command.in?, pivot.motor.command.out!)
check.has.space(sonar.data?, inhibit!)
: