Priorities

Avoid beats seek: behaviours that take over and run to completion.

5.4BehavioursRobot club20 min

A robot wants several things at once: reach the goal, do not hit anything, do not fall off the table. They conflict. The classic answer, from the 1980s and still in use, is to give each behaviour a priority and let the most urgent one that has something to say take over.

Seek alone

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

# maths: atan2, hypot, sin, cos, radians
import math

def wrapped(h):
    return (h + 180) % 360 - 180

def steer_to(x, y, speed=60):
    # where am I?
    px, py = position()
    bearing = math.degrees(math.atan2(x - px, y - py))
    error = wrapped(bearing - heading())
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(speed if abs(error) < 25 else 0, 0, max(-60, min(60, error * 3)))

def near(x, y, cm=6):
    # where am I?
    px, py = position()
    return math.hypot(x - px, y - py) < cm

# relative to the start at (15, 15)
goal = (82 - 15, 82 - 15)
# do this 150 times (tick counts from 0)
for tick in range(150):
    if near(*goal, cm=5):
        # leave the loop
        break
    steer_to(*goal, speed=60)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()
print("ended at", position())

Run this in the simulator

steer_to turns to face the point and drives, so the distance sensor looks where the robot is going. The seek is single-minded: it gets there in the end, but only by scraping along both walls on the way. The task marks that as a failure, and on a real mat a scraping robot is a stuck robot.

Avoid takes over

Add a second behaviour, avoid, with a higher priority. When the sensor sees something close, avoid takes the robot completely for a few ticks: back off and slide sideways. Then it lets go, and seek carries on:

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

# maths: atan2, hypot, sin, cos, radians
import math

def wrapped(h):
    return (h + 180) % 360 - 180

def steer_to(x, y, speed=60):
    # where am I?
    px, py = position()
    bearing = math.degrees(math.atan2(x - px, y - py))
    error = wrapped(bearing - heading())
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(speed if abs(error) < 25 else 0, 0, max(-60, min(60, error * 3)))

def near(x, y, cm=6):
    # where am I?
    px, py = position()
    return math.hypot(x - px, y - py) < cm

goal = (82 - 15, 82 - 15)
state = "seek"
avoid_ticks = 0
# do this 390 times (tick counts from 0)
for tick in range(390):
    if near(*goal, cm=5):
        # leave the loop
        break
    if state == "seek" and distance() < 22:
        # avoiding takes over completely...
        state = "avoid"
        avoid_ticks = 0
    if state == "avoid":
        # ...back a touch and slide right
        drive(-10, 60, 0)
        avoid_ticks += 1
        if avoid_ticks >= 10:
            # ...then seeking gets the robot back
            state = "seek"
    else:
        steer_to(*goal, speed=60)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()
print("ended at", position(), "after", tick, "ticks")

Run this in the simulator

Two things make this work. Avoid runs to completion: ten ticks, whatever the sensor says, so the robot actually gets clear instead of flickering between the two. And seek does not know avoid exists: it just finds itself somewhere new and heads for the goal again.

The rule

Order the behaviours by urgency. Each tick, the first one that wants control gets it:

if emergency():        # highest priority
    ...
elif avoiding():
    ...
else:                  # lowest priority: the actual job
    seek()

This is called subsumption: higher layers subsume the lower ones. The lower layers are simple and always there; the higher ones only speak up when they must. You can add a layer without touching the ones below.

Task: seek, but safely

Reach the green zone at (82, 82) without touching either wall.

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

# maths: atan2, hypot, sin, cos, radians
import math

def wrapped(h):
    return (h + 180) % 360 - 180

def steer_to(x, y, speed=60):
    # where am I?
    px, py = position()
    bearing = math.degrees(math.atan2(x - px, y - py))
    error = wrapped(bearing - heading())
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(speed if abs(error) < 25 else 0, 0, max(-60, min(60, error * 3)))

def near(x, y, cm=6):
    # where am I?
    px, py = position()
    return math.hypot(x - px, y - py) < cm

goal = (82 - 15, 82 - 15)
# do this 390 times (tick counts from 0)
for tick in range(390):
    if near(*goal, cm=5):
        # leave the loop
        break
    steer_to(*goal, speed=60)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()

Challenges

Slide left or right depending on which side of the depth grid has more room.
Add a third layer above avoid: if the battery is under 20, stop and print a warning. (Change the number to test it.)
Count how many times avoid took over, and print it at the end.