Go to a point

The first useful controller: a vector to the target, straight through the inverse kinematics.

U2.6Kinematics and framesUniversity25 min

With the inverse kinematics in hand, the first genuinely useful controller is short.

each tick:
    vector to the target, in the world
    speed proportional to how far away it is, capped
    that velocity, through the inverse kinematics
    stop when close enough

from bugbot import *
import math
connect()

V_MAX, V_LAT = 20.0, 15.0
TX, TY = 60.0, 70.0          # cm from where the robot started

for tick in range(300):
    x, y = position()
    dx, dy = TX - x, TY - y
    gap = math.hypot(dx, dy)
    if gap < 4:
        break
    speed = min(14.0, 0.5 * gap)             # proportional, with a ceiling
    wx, wy = speed * dx / gap, speed * dy / gap
    a = math.radians(heading())
    drive(100 * (wx * math.sin(a) + wy * math.cos(a)) / V_MAX,
          100 * (wx * math.cos(a) - wy * math.sin(a)) / V_LAT, 0)
    wait(0.1)
stop()
print("arrived at", position())

Run this in the simulator

What is going on

speed = min(14, 0.5 * gap) is proportional control on distance, which you will meet properly in U5. Far away, it drives at its ceiling. Close in, it eases off, so the robot does not overshoot and hunt about the target.

Dividing dx, dy by gap makes a unit vector: direction without magnitude. Multiplying by speed puts the magnitude back. Doing it in one step, speed * dx, is a common slip that makes the gain depend on how far away the target is, so the robot behaves differently at the start of a long run than a short one.

Where the position came from

This controller reads position(), and U1.3 said that is the lab's overhead camera, not the robot's. It is fine while the subject is kinematics. From U6 onwards, position() is replaced by an estimate the robot works out for itself, and this same loop keeps running on top of it. That is why it is worth writing the loop cleanly now: everything later plugs into it.

Stopping

gap < 4 sounds arbitrary and is not. It has to be bigger than the robot's coasting distance from the speed it arrives at, or the robot will sail past, turn round, come back and hunt. Measure your coast, then pick the threshold. Alternatively, ramp the speed down far enough that it arrives slowly, which is gentler but slower.

Task: go to a point

Drive to world (140, 150) and stop within 10 cm of it. The robot starts at (60, 60) facing the wrong way, and position() counts from where it started.

from bugbot import *
import math
connect()

V_MAX, V_LAT = 20.0, 15.0
TX, TY = 80.0, 90.0        # the target, in cm from the start

Challenges

Add a heading term so the robot also finishes facing a chosen direction, without changing the path.
Feed it a list of points and drive them in order, without stopping at each one.
Replace position() with odometry() and run it again. How close does it get now, and how much worse does it get on a longer run?