Project: rescue
Search, approach, return: camera, servoing and states in one machine.
Do this lesson in the simulatorMarker 9 is somewhere on the mat. Find it with the camera, go to it, and bring the news home, without hitting the box on the way. Search, approach and return: a three-state machine using the camera from Module 4, the servoing from Module 3, and everything in this module.
The plan, drawn
search --(tag 9 in view)--> approach --(closer than 12 cm)--> home --(near the start)--> done
Three states, three transitions, each with a condition you can read straight off the sensors.
Search and approach
# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()
def bearing_of(cx):
return (cx - 160) * 120 / 320
# camera: the tag detector
set_cv("apriltag")
state = "search"
# do this 300 times (tick counts from 0)
for tick in range(300):
tags = [t for t in apriltags() if t[0] == 9]
if state == "search":
if tags:
print("found 9 at tick", tick)
state = "approach"
else:
# spin clockwise on the spot at 40
turn_right(40)
elif state == "approach":
if tags and tags[0][3] < 12:
# leave the loop
break
elif tags:
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(60, 0, bearing_of(tags[0][1]) * 3)
else:
# spin clockwise on the spot at 30
turn_right(30)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
print("at the marker:", position())
Home
The start is (0, 0) in relative coordinates, so going home is go_to(0, 0) until near(0, 0). Add the state, and the machine is complete:
# 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 go_to(x, y, speed=70):
# where am I?
px, py = position()
a = math.radians(wrapped(math.degrees(math.atan2(x - px, y - py)) - heading()))
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(speed * math.cos(a), speed * math.sin(a), wrapped(0 - heading()) * 3)
def near(x, y, cm=6):
# where am I?
px, py = position()
return math.hypot(x - px, y - py) < cm
def bearing_of(cx):
return (cx - 160) * 120 / 320
# camera: the tag detector
set_cv("apriltag")
state = "search"
# do this 590 times (tick counts from 0)
for tick in range(590):
tags = [t for t in apriltags() if t[0] == 9]
if state == "search":
if tags:
print("found 9")
state = "approach"
else:
# spin clockwise on the spot at 40
turn_right(40)
elif state == "approach":
if tags and tags[0][3] < 12:
state = "home"
elif tags:
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(60, 0, bearing_of(tags[0][1]) * 3)
else:
# spin clockwise on the spot at 30
turn_right(30)
elif state == "home":
if near(0, 0, cm=8):
# leave the loop
break
go_to(0, 0, speed=60)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
print("home:", position(), "after", tick / 10, "seconds")
What you have built
Look at the shape of that program: helpers at the top, one state variable, one loop, one if / elif per state, transitions as plain conditions. That is how behaviour code is written for robots that cost a million pounds, and it is exactly what you have. The only differences are more states and more sensors.
Task: rescue
Find marker 9, print found 9, drive to it, and return home, without touching the box.
# 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 go_to(x, y, speed=70):
# where am I?
px, py = position()
a = math.radians(wrapped(math.degrees(math.atan2(x - px, y - py)) - heading()))
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(speed * math.cos(a), speed * math.sin(a), wrapped(0 - heading()) * 3)
def near(x, y, cm=6):
# where am I?
px, py = position()
return math.hypot(x - px, y - py) < cm
def bearing_of(cx):
return (cx - 160) * 120 / 320
# camera: the tag detector
set_cv('apriltag')
state = 'search'
# do this 590 times (tick counts from 0)
for tick in range(590):
tags = [t for t in apriltags() if t[0] == 9]
if state == 'search':
if tags:
print('found 9')
state = 'approach'
else:
# spin clockwise on the spot at 40
turn_right(40)
elif state == 'approach':
if tags and tags[0][3] < 12:
# leave the loop
break
elif tags:
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(60, 0, bearing_of(tags[0][1]) * 3)
else:
# spin clockwise on the spot at 30
turn_right(30)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
Where next
That is the end of the course as it stands. Firefighter, Shrinking Circle and Control Points are the competitions for this module. After that: the real robot. Everything you have written runs on a BugBot unchanged, and the mat will not be quite as tidy as the simulator's.
Challenges
- Add an "avoid" layer from lesson 5.4 above all three states, and move the box into the way.
- Rescue two markers, 9 and then 8 (add one to the scene in the free-play editor), and come home once.
- Blink the LED in a different pattern in each state, so a person can tell what the robot is thinking.