A5.9 Project: plan the route, then drive it

The starter has the map and the go_to helper. Write the rest: - Build an undirected weighted graph from WAYPOINTS and ROADS, where each road's weight is its length in cm (use math.dist; do not type the lengths). - Write dijkstra(graph, start) and use it to find the shortest route from S to G. The program must find the route: do not type it. - Print route: <waypoints joined with ->, such as route: S-X-Y-G, and length: <n> cm, with n a whole number. - Drive the route by calling go_to for each waypoint after S, in order. Stay on the roads: do not go near waypoints that are not on your route, and do not touch a wall. - When the robot reaches G, turn the LED green.

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

import math
import heapq

WAYPOINTS = {"S": (10, 10), "A": (10, 90), "B": (90, 90), "C": (35, 10), "D": (35, 35),
             "E": (65, 35), "F": (65, 60), "G": (90, 60), "J": (80, 35)}
ROADS = [("S", "A"), ("A", "B"), ("B", "G"), ("S", "C"), ("C", "D"),
         ("D", "E"), ("E", "F"), ("F", "G"), ("E", "J")]
START = WAYPOINTS["S"]              # the robot starts at S

def go_to(x, y):
    """Drive to the mat point (x, y): along y, then along x, twice so the second pass corrects any drift."""
    for attempt in range(2):
        px, py = position()
        dy = y - (START[1] + py)
        if dy > 1:
            forward(60, distance=dy)
        elif dy < -1:
            backward(60, distance=-dy)
        px, py = position()
        dx = x - (START[0] + px)
        if dx > 1:
            right(60, distance=dx)
        elif dx < -1:
            left(60, distance=-dx)

The hint students can ask for: Make an empty neighbour dictionary for every waypoint, then add each road to both of its ends with its length. Run Dijkstra's algorithm from S, follow the previous waypoints back from G and reverse them. Print the route, then go_to each waypoint after S in that order, and light the LED at the end.

A solution

from bugbot import *
connect()

import math
import heapq

WAYPOINTS = {"S": (10, 10), "A": (10, 90), "B": (90, 90), "C": (35, 10), "D": (35, 35),
             "E": (65, 35), "F": (65, 60), "G": (90, 60), "J": (80, 35)}
ROADS = [("S", "A"), ("A", "B"), ("B", "G"), ("S", "C"), ("C", "D"),
         ("D", "E"), ("E", "F"), ("F", "G"), ("E", "J")]
START = WAYPOINTS["S"]

def go_to(x, y):
    for attempt in range(2):
        px, py = position()
        dy = y - (START[1] + py)
        if dy > 1:
            forward(60, distance=dy)
        elif dy < -1:
            backward(60, distance=-dy)
        px, py = position()
        dx = x - (START[0] + px)
        if dx > 1:
            right(60, distance=dx)
        elif dx < -1:
            left(60, distance=-dx)

graph = {name: {} for name in WAYPOINTS}
for a, b in ROADS:
    length = math.dist(WAYPOINTS[a], WAYPOINTS[b])
    graph[a][b] = length
    graph[b][a] = length

def dijkstra(graph, start):
    distance = {v: float("inf") for v in graph}
    previous = {v: None for v in graph}
    distance[start] = 0
    visited = set()
    queue = [(0, start)]
    while queue:
        d, u = heapq.heappop(queue)
        if u in visited:
            continue
        visited.add(u)
        for v, w in graph[u].items():
            if v not in visited and d + w < distance[v]:
                distance[v] = d + w
                previous[v] = u
                heapq.heappush(queue, (d + w, v))
    return distance, previous

distance, previous = dijkstra(graph, "S")
route, v = [], "G"
while v is not None:
    route.append(v)
    v = previous[v]
route.reverse()
print("route: " + "-".join(route))
print("length:", round(distance["G"]), "cm")
for name in route[1:]:
    go_to(*WAYPOINTS[name])
led("green")

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