U3.8 Twists and the exponential

The green square is 25 cm to the robot's right and 30 cm ahead of it. The robot must arrive in it facing 120 degrees (a third of a turn clockwise), using one steady drive() command held for T = 3 seconds, then stop(). Work out the twist with the logarithm, print it as a line starting twist: with w = ..., vx = ... and vy = ... (deg/s and cm/s), turn it into a command with the full-scale speeds below, and drive it. The printed twist is checked against the logarithm, to a tenth.

from bugbot import *
import math
connect()

V_MAX, V_LAT, W_MAX = 20.0, 15.0, 120.0     # the design's full-scale speeds: cm/s forward, cm/s sideways, deg/s
DX, DY, TURN = 25.0, 30.0, 120.0            # where to end up, in the start's body frame, and how far to turn
T = 3.0                                     # how long to hold the command, s

The hint students can ask for: Work out w from the turn and the time, in radians a second, then C and S, then vx and vy from the logarithm. Print them. Divide each speed by its full scale to get a percentage, check none sits inside the 15 percent dead band or above 100, and hold that one drive() for exactly T before stopping.

A solution

from bugbot import *
import math
connect()

V_MAX, V_LAT, W_MAX = 20.0, 15.0, 120.0
DX, DY, TURN = 25.0, 30.0, 120.0
T = 3.0

w = math.radians(TURN) / T                    # rad/s
C = math.sin(w * T) / w
S = (1 - math.cos(w * T)) / w
n = C * C + S * S
vx = (C * DX - S * DY) / n
vy = (S * DX + C * DY) / n
print("twist: w =", round(math.degrees(w), 2), "deg/s, vx =", round(vx, 2), "vy =", round(vy, 2), "cm/s")

cmd = (100 * vy / V_MAX, 100 * vx / V_LAT, 100 * math.degrees(w) / W_MAX)
print("drive", [round(c, 1) for c in cmd])
drive(*cmd)
wait(T)
stop()

Any program that meets the task's checks is marked correct in the simulator; this is one way, not the only way.