Project: the delivery
Follow a line to a marker and back through traffic.
Do this lesson in the simulatorFollow the line to marker 7, report it, turn round, and follow the line home, while robot 102 shuttles across the route. Lines, tags, a state machine, and giving way, all in one program.
The plan, drawn
out --(marker 7 closer than 20 cm)--> turn
turn --(facing back, line in view)--> home
home --(in the home zone)--> done
And one rule above all three states: if robot 102 is close ahead, stop until it has gone.
Two detectors, one camera
The camera runs one detector at a time. Following the line needs line; spotting the marker and the crossing robot needs apriltag. Switch between them inside the tick:
# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()
# camera: the line detector
set_cv("line")
# do this 3 times (i counts from 0)
for i in range(3):
# camera: the tag detector
set_cv("apriltag")
# every tag in view: [id, cx, cy, distance], nearest first
tags = apriltags()
# camera: the line detector
set_cv("line")
print("tags:", [t[0] for t in tags], "line:", line())
# drive forward at 50 for 10 cm, then stop
forward(50, distance=10)
Switching costs a frame on the real robot, which is fine at ten ticks a second. Keep the line detector as the default and borrow the tag detector briefly.
Turning round
At the marker the robot has to face back the way it came before it can follow the line home. Remember the heading when the marker was found, and spin until the heading is 180 from it and the line is back in view:
# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()
# camera: the line detector
set_cv("line")
# get onto the line first
forward(50, distance=30)
# which way I face: degrees clockwise from 0
h0 = heading()
# do this 60 times (tick counts from 0)
for tick in range(60):
# spin clockwise on the spot at 60
turn_right(60)
facing_back = abs((heading() - h0 - 180 + 180) % 360 - 180) < 20
if facing_back and line():
# leave the loop
break
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
print("turned from", round(h0), "to", round(heading()), "line:", line())
The awkward expression is just wrapped(heading() - h0 - 180) from Module 3: how far the current heading is from the opposite of the start.
The whole program
# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()
def follow_step(speed=60, gain=0.4):
# [cx, angle] of the line ahead, or [] if none
seen = line()
if not seen:
return False
cx, angle = seen
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(speed, 0, (cx - 160) * gain)
return True
# camera: the line detector
set_cv("line")
state = "out"
# do this 690 times (tick counts from 0)
for tick in range(690):
# where am I? (cm from where I started)
x, y = position()
if state == "out":
# camera: the tag detector
set_cv("apriltag")
tags = [t for t in apriltags() if t[0] == 7]
# camera: the line detector
set_cv("line")
if tags and tags[0][3] < 20:
print("found 7")
state = "turn"
# which way I face: degrees clockwise from 0
h0 = heading()
elif not follow_step(55, 0.4):
# drive forward at 30 (keeps going until the next command)
forward(30)
elif state == "turn":
# spin clockwise on the spot at 60
turn_right(60)
facing_back = abs((heading() - h0 - 180 + 180) % 360 - 180) < 20
if facing_back and line():
state = "home"
elif state == "home":
if x < -20 and y < 25:
# leave the loop
break
if not follow_step(55, 0.4):
# drive forward at 30 (keeps going until the next command)
forward(30)
# the rule above the states: give way to robot 102 when it is close ahead
# camera: the tag detector
set_cv("apriltag")
others = [t for t in apriltags() if t[0] == 102 and t[3] < 25]
# camera: the line detector
set_cv("line")
if others:
# all motors off
stop()
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
print("home:", position())
The give-way rule sits after the state logic so it overrides whatever the state asked for. That is the subsumption idea from lesson 5.4 in three lines.
Task: the delivery
Follow the line to marker 7, print found 7, and follow it home without touching robot 102.
# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()
def follow_step(speed=60, gain=0.4):
# [cx, angle] of the line ahead, or [] if none
seen = line()
if not seen:
return False
cx, angle = seen
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(speed, 0, (cx - 160) * gain)
return True
# camera: the line detector
set_cv("line")
# again and again, for ever
while True:
if not follow_step(55, 0.4):
# leave the loop
break
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
Where next
The Line race competition puts you on the same loop as three other robots, and Light Cycles turns lines into walls. Module 7 gives the robot a gripper and a kicker, and a ball to use them on.
Challenges
- Make the give-way rule use the prediction from lesson 6.7 instead of stopping for every close robot.
- Blink the LED green on the way out and blue on the way home.
- Time the whole delivery and print it. Then make it faster on the straights.