Project: clear the mat

Every ball into the home zone.

7.7Hands and feetRobot club30 min

Three balls somewhere on the mat, one home zone, a robot with a gripper. Bring them all in. It is fetch from lesson 7.3 in a loop, plus the small things that stop a loop like that from working: not pushing your earlier balls back out, and not fetching the same ball twice.

The loop

for ball in range(3):
    find_ball("red")
    fetch("red")
    go home
    # open the jaws and let go
    gripper("open")
    back away

find_ball spins until a red ball is in view. Once a ball is in the home zone it is still red, so on later trips the search can find that one first: the section below deals with that. The step back and the step aside at the end of each trip keep the robot clear of the ball it just dropped.

Helpers

All from earlier lessons:

# 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')
print("first ball:", fetch('red'), holding())
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")
# let go and step back
backward(50, distance=8)
# step aside, clear of the dropped ball
right(50, distance=14)
# and away from home before the next search
forward(50, distance=20)
print("one home, at", position())

Run this in the simulator

Where it goes wrong

Run the loop three times and watch. Two things bite. After the first trip there is a red ball sitting in the home zone, and find_ball is just as happy to find that one again. And when the robot drops a ball and then drives off, it can shove the ball it just dropped.

The first is fixed by working out where each blob is on the mat, from its bearing and its width, and ignoring the ones that are already home:

# 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 bearing_of(cx):
    return (cx - 160) * 120 / 320

def balls_out(colour):
    # the balls of this colour in view that are NOT already in the home zone
    out = []
    # where am I?
    px, py = position()
    for b in blobs():
        width = b[5] - b[3]
        # a 4 cm ball, `width` pixels wide
        dist = 4 * 92 / max(width, 1)
        h = math.radians(heading() + bearing_of(b[0]))
        bx, by = px + math.sin(h) * dist, py + math.cos(h) * dist
        # home is the zone around the start
        if not (abs(bx) < 15 and by < 16):
            out.append(b)
    return out

# camera: look for red blobs
set_cv("blob", "red")
print("balls still out:", len(balls_out("red")), "of", len(blobs()), "in view")

Run this in the simulator

The second is fixed by stepping back, then sideways, before driving off, so the robot's body never passes through the spot where it left the ball. Use balls_out in place of blobs() inside find_ball and fetch, and only grip when one of those is close.

Task: clear the mat

Bring all three red balls into the home zone within two minutes.

# 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')
while not near(0, 0, cm=6):
    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()
# open the jaws and let go
gripper("open")

Where next

The Netball competition brings in the gripper, the kicker and passing, against another team, with no contact allowed. The game programs are written exactly like these lessons. Module 8 starts teaching the robot instead of programming it.

Challenges

Print the time each ball took.
Fetch the nearest ball first: compare the blob widths before choosing.
Add a fourth ball in the free-play editor and clear all four.