U9.9 Planning for a car

The green bay is on the other side of the wall, a 30 cm square round the goal, and the car must finish in it facing back down the mat (180 degrees, within 25). Driving one plan to the end misses it, as above. Write plan(), an A* search over the car's three moves, then a loop that plans from where the robot really is, drives only the first move with car(), and plans again. Stop when the plan comes back empty, and say so if it comes back with no path at all: those are different endings. No sliding, no turning on the spot, no touching the wall, and no list of moves typed in by hand. free(), arc(), here() and car() are written for you.

from bugbot import *
import heapq, math
connect()

V_MAX, W_MAX, R_MIN = 20.0, 120.0, 25.0
SPEED, STEP_S = 60, 1.6                    # every move: 60 percent for 1.6 s, about 19 cm on paper
INFLATE = 9.0
START = (50.0, 40.0)
GOAL, GOAL_H = (155.0, 40.0), 180.0        # the far side of the wall, facing back down the mat
WALLS = [(95.0, 0.0, 10.0, 120.0)]
V_SEEN = SPEED / 100 * V_MAX               # the speed the model uses; car() replaces it with the one it measures

def free(x, y):
    if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
        return False
    return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
                   for ox, oy, ow, oh in WALLS)

def arc(x, y, h, steer, seconds, n=8):
    """Where car(SPEED, steer, seconds) goes at V_SEEN on a circle of R_MIN, as n points along the way."""
    v = V_SEEN
    w = math.degrees(v / R_MIN) * steer
    pts = []
    for k in range(n):
        dt = seconds / n
        a, wr = math.radians(h), math.radians(w)
        if abs(wr) < 1e-9:
            C, S = math.cos(a) * dt, math.sin(a) * dt
        else:
            C = (math.sin(a + wr * dt) - math.sin(a)) / wr
            S = (math.cos(a) - math.cos(a + wr * dt)) / wr
        x, y, h = x + v * S, y + v * C, (h + w * dt) % 360
        pts.append((x, y, h))
    return pts

def here():
    px, py = position()
    return START[0] + px, START[1] + py, heading()

def car(speed, steer, seconds, dt=0.1):
    """Drive at speed, steer -1..1, and every dt set the turn rate from the speed measured, so the radius stays R_MIN."""
    global V_SEEN
    speed, steer = max(-100, min(100, speed)), max(-1, min(1, steer))       # top speed and full lock, as in U2.8
    rot = 100 * math.degrees(speed / 100 * V_MAX / R_MIN) * steer / W_MAX      # first guess, from the nominal speed
    for k in range(round(seconds / dt)):
        (x0, y0), h0 = position(), heading()
        drive(speed, 0, rot)
        wait(dt)
        (x1, y1), h1 = position(), heading()
        v = math.copysign(math.hypot(x1 - x0, y1 - y0) / dt, speed)
        w = ((h1 - h0 + 180) % 360 - 180) / dt
        rot += 0.5 * (math.degrees(v / R_MIN) * steer - w) * 100 / W_MAX
        if k >= 4:                                                            # once it is up to speed
            V_SEEN += 0.2 * (abs(v) - V_SEEN)

# write plan(): A* over the car's three moves

# then the loop: plan from here(), drive only the first move, and plan again

Plan your program here, then type it in and press Run.

Do it on the robot
www.bugbotlab.com/learn/u9-9-planning-for-a-car/
The simulator checks it and tells you when it passes. Nothing to install, no account.