Answers

4.3 Visual servoing

Vision · Robot club · about 20 min

What this lesson is about

Steer from cx, speed from distance: docking at a tag.

• Two errors from one tag
• Steer while driving
• Speed from distance
• When the tag disappears

Questions 6 marks in all

1. [1 mark]A tag gives two errors at once. Which pairing does the docking loop use?

   1. Acx sets the rotation, distance sets the forward speed
   2. Bcx sets the forward speed, distance sets the rotation
   3. Ccy sets the rotation, id sets the speed
   4. Ddistance sets both
   Answer: A. Off-centre means turn, far away means drive. Two errors, two outputs, every tick.

2. [1 mark]What does this program print?

   stop_at = 13
   for dist in [40, 20, 14]:
       speed = max(20, min(70, (dist - stop_at) * 3))
       print(speed)

   Answer:

   70
   21
   20

   40 gives 81, clamped down to 70. 20 gives 21. 14 gives 3, clamped up to 20.

3. [1 mark]With stop_at = 13 and speed = max(20, min(70, (dist - stop_at) * 3)), what speed does the robot use when the tag is 25 cm away?

   Answer: 36. (25 - 13) * 3 is 36, which is between 20 and 70, so the clamp leaves it alone.

4. [1 mark]Why does the speed have a lower limit of 20, the max(20, ...)?

   1. ASo the robot does not crawl to a halt before it reaches the stopping distance
   2. BSo it never drives faster than 20
   3. CSo it can reverse if it gets too close
   4. DBecause the camera cannot see below 20 cm
   Answer: A. Close to the tag, (dist - stop_at) * 3 is tiny, too small to move the robot. The minimum keeps it creeping in.

5. [1 mark]Near the tag, the camera loses it for one frame and the loop does if not tags: break. Why is that a poor choice?

   1. AThe robot stops dead even though the tag is almost certainly still there
   2. Bbreak restarts the loop from the top
   3. CIt makes the robot spin forever
   4. DIt is an error to break out of a while loop
   Answer: A. Losing a tag for a frame is normal. Better to keep going for a few ticks, or turn slowly towards where it was last seen.

6. [1 mark]Put one tick of the visual servoing loop in order.

   Number the lines 1 to 4 to put them in the right order.

   1. Read the tags with `apriltags()`
   2. Work out the bearing error and the distance error
   3. Send one `drive` with a speed and a rotation
   4. Wait 0.1 s, then go round again
   Answer:

   Read the tags with `apriltags()`
   Work out the bearing error and the distance error
   Send one `drive` with a speed and a rotation
   Wait 0.1 s, then go round again

   Read, two errors, two outputs, repeat. That is the whole lesson in four lines.

The task: dock at the marker

# 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:
        # leave the loop
        break
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(50, 0, bearing_of(tags[0][1]) * 3)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()

from bugbot import *
connect()
def bearing_of(cx):
    return (cx - 160) * 120 / 320      # pixels to degrees, 120 degree view across 320 pixels
def find(tag_id):
    # spin until the tag is in view, then square up to it
    while True:
        tags = [t for t in apriltags() if t[0] == tag_id]
        if tags:
            break
        turn_right(40)
        wait(0.1)
    stop()
    wait(0.3)

def approach(tag_id, stop_at):
    while True:
        tags = [t for t in apriltags() if t[0] == tag_id]
        if not tags:
            turn_right(30)
            wait(0.1)
            continue
        tag = tags[0]
        cx, dist = tag[1], tag[3]
        if dist <= stop_at:
            break
        rot = bearing_of(cx) * 3
        speed = max(20, min(70, (dist - stop_at) * 3))
        drive(speed, 0, rot)
        wait(0.1)
    stop()
set_cv('apriltag')
approach(1, 13)
err = (0 - heading() + 180) % 360 - 180      # square up: the card faces down the mat, so facing it is heading 0
turn_right(30, angle=err)