Fetching a ball
See it, approach it, grip it, bring it home.
Do this lesson in the simulatorFind it, go to it, grip it, bring it home. Every part of that is something you have done: the blob detector from 4.4, servoing from 4.3, the gripper from 7.2, and go_to from Module 5. This lesson joins them into fetch, a function you will use for the rest of the module.
Find it
# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()
# camera: look for red blobs
set_cv("blob", "red")
# until a blob of that colour is in view
while not blobs():
# spin clockwise on the spot at 40
turn_right(40)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
# pause 0.3 s (the robot keeps doing what it was told)
wait(0.3)
print("red ball at cx", blobs()[0][0], "after turning to", round(heading()))
The search from lesson 4.5, aimed at a colour instead of a tag.
Approach and grip
Steer from the blob's cx, slow down as the distance sensor reading falls, and grip when the ball is in reach. If the grip misses, creep a little closer and try again:
# 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
def fetch(colour):
# approach the ball with the camera, slow down close to it, then grip; returns True when holding it
set_cv('blob', colour)
# do this 250 times (tick counts from 0)
for tick in range(250):
# the blobs of the chosen colour, largest first
found = blobs()
# from the robot's centre: the front edge is 3.5 cm out
d = distance()
if d < 5.5:
# all motors off
stop()
# close the jaws (it waits while they move)
gripper("close")
if holding():
return True
# not quite in reach: a little closer
forward(15)
elif found:
error = bearing_of(found[0][0])
speed = 15 if d < 10 else (45 if abs(error) < 10 else 20)
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(speed, 0, error * 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()
return False
# camera: look for red blobs
set_cv("blob", "red")
# until a blob of that colour is in view
while not blobs():
# spin clockwise on the spot at 40
turn_right(40)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
print("fetched:", fetch("red"), "holding:", holding())
Three speeds: 45 when the ball is centred and far, 20 while still turning towards it, 15 for the last few centimetres. The slow finish is what makes the grip reliable.
Bring it home
go_to from lesson 5.2 slides the robot back to the start while holding its heading. The ball rides in front the whole way. Then let go:
# 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=60):
# 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=5):
# where am I?
px, py = position()
return math.hypot(x - px, y - py) < cm
# drive forward at 50 for 20 cm, then stop
forward(50, distance=20)
# slide right at 50 for 15 cm, then stop
right(50, distance=15)
print("pretend we fetched something out here:", position())
while not near(0, 0, cm=6):
go_to(0, 0)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
# open the jaws and let go
gripper("open")
print("home:", position())
Task: fetch the ball
Find the red ball, fetch it, and leave it in the home zone.
# 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 bearing_of(cx):
return (cx - 160) * 120 / 320
def go_to(x, y, speed=60):
# 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=5):
# where am I?
px, py = position()
return math.hypot(x - px, y - py) < cm
def find_ball(colour):
set_cv('blob', colour)
# until a blob of that colour is in view
while not blobs():
# spin clockwise on the spot at 40
turn_right(40)
# pause 0.1 s (the robot keeps doing what it was told)
wait(0.1)
# all motors off
stop()
# pause 0.3 s (the robot keeps doing what it was told)
wait(0.3)
def fetch(colour):
set_cv('blob', colour)
# do this 250 times (tick counts from 0)
for tick in range(250):
# the blobs of the chosen colour, largest first
found = blobs()
# cm to the nearest thing ahead
d = distance()
if d < 5.5:
# all motors off
stop()
# close the jaws (it waits while they move)
gripper("close")
if holding():
return True
# drive forward at 15 (keeps going until the next command)
forward(15)
elif found:
error = bearing_of(found[0][0])
speed = 15 if d < 10 else (45 if abs(error) < 10 else 20)
# forward, sideways, rotation: -100 to 100 each, until the next command
drive(speed, 0, error * 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()
return False
find_ball('red')
fetch('red')
print('holding:', holding())
Challenges
- Print how long the fetch took.
- Fetch a blue ball instead. (Add one in the free-play editor first.)
- Make
fetchgive up after 10 seconds and returnFalseinstead of searching forever.