Where is it?

Pixels to degrees, and a controller that turns to face a tag.

4.2VisionRobot club15 min

Do this lesson in the simulator

A tag at cx = 160 is straight ahead. A tag at cx = 300 is off to the right. The camera turns pixels into a direction, and that is all a robot needs to steer.

Pixels to degrees

The camera sees 120 degrees from the left edge of the picture to the right, across 320 pixels: a wide-angle lens. So each pixel is 120 / 320 of a degree, and the bearing of a tag is:

bearing = (cx - 160) * 120 / 320       # degrees, positive to the right

# 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")
tag = apriltags()[0]
print("cx:", tag[1], "bearing:", round(bearing_of(tag[1]), 1), "degrees")

Run this in the simulator

Marker 3 is about 20 degrees to the right. That is not quite exact, because a camera lens is not a perfect ruler, but it is within a degree or two across the whole picture, and the controller you are about to write does not care.

Turn to face it

The bearing is an error, and you know what to do with an error: a proportional controller on the rotation.

# 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")
# do this 30 times (tick counts from 0)
for tick in range(30):
    # every tag in view: [id, cx, cy, distance], nearest first
    tags = apriltags()
    if not tags:
        # leave the loop
        break
    error = bearing_of(tags[0][1])
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(0, 0, error * 3)
    # pause 0.05 s (the robot keeps doing what it was told)
    wait(0.05)
# all motors off
stop()
# pause 0.3 s (the robot keeps doing what it was told)
wait(0.3)
tag = apriltags()[0]
print("now cx =", tag[1], "and I am facing", round(heading()))

Run this in the simulator

The tag slides to the middle of the picture, and the heading has changed by about the bearing you printed. The camera never told you the heading. It told you the error, and the loop drove it to zero.

Stopping cleanly

The loop above just runs 30 times. Better to stop when the error is small, then settle and check, exactly as in lesson 3.1:

# 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")
# again and again, for ever
while True:
    # every tag in view: [id, cx, cy, distance], nearest first
    tags = apriltags()
    if not tags:
        # 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)
        # back to the top of the loop
        continue
    error = bearing_of(tags[0][1])
    if abs(error) < 1.5:
        # all motors off
        stop()
        # pause 0.3 s (the robot keeps doing what it was told)
        wait(0.3)
        if abs(bearing_of(apriltags()[0][1])) < 1.5:
            # leave the loop
            break
        # back to the top of the loop
        continue
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(0, 0, max(-30, min(30, error * 3)))
    # pause 0.05 s (the robot keeps doing what it was told)
    wait(0.05)
# all motors off
stop()
print("centred, facing", round(heading()))

Run this in the simulator

The if not tags branch keeps spinning if the tag is out of view. It is the beginning of a search, which lesson 4.5 finishes.

Task: face the marker

Turn until marker 3 is in the middle of the picture, then print centred. Do not drive.

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

# camera: the tag detector
set_cv("apriltag")
print(apriltags())

Challenges

  1. Print the heading before and after, and compare the change with the bearing you computed.
  2. Face the marker, then turn 180 away from it and face it again.
  3. Work out the bearing with math.atan((cx - 160) / 92) instead. How different is it at the edge of the picture?