from XRPLib.reflectance import Reflectance
from XRPLib.differential_drive import DifferentialDrive
from XRPLib.board import Board
from XRPLib.rangefinder import Rangefinder
import time
import random
reflectance = Reflectance.get_default_reflectance()
drivetrain = DifferentialDrive.get_default_differential_drive()
board = Board.get_default_board()
rangefinder = Rangefinder.get_default_rangefinder()
class LineSensor:
def __init__(self):
self.reflectance = Reflectance.get_default_reflectance()
self.threshold = 0.5
def get_left(self):
return self.reflectance.get_left()
def get_right(self):
return self.reflectance.get_right()
def get_error(self):
return self.get_left() - self.get_right()
def is_at_cross(self):
return self.get_left() > self.threshold and self.get_right() > self.threshold
def is_either_on_line(self):
return self.get_left() > self.threshold or self.get_right() > self.threshold
sensor = LineSensor()
def test_sensors():
for i in range(50):
print("Left:", round(sensor.get_left(), 2), " Right:", round(sensor.get_right(), 2),
" Error:", round(sensor.get_error(), 2),
" On line:", sensor.is_either_on_line(), " Cross:", sensor.is_at_cross())
time.sleep(0.1)
def test_drive_to_line():
while sensor.get_left() < sensor.threshold and sensor.get_right() < sensor.threshold:
drivetrain.set_effort(0.3, 0.3)
drivetrain.stop()
print("Line detected!")
def test_bounce():
bounce_count = 0
while True:
if sensor.is_either_on_line():
drivetrain.stop()
bounce_count = bounce_count + 1
print("Bounce", bounce_count)
drivetrain.turn(180)
else:
drivetrain.set_effort(0.3, 0.3)
def test_random_bounce():
speed = 0.3
while True:
if sensor.is_either_on_line():
drivetrain.stop()
angle = random.randint(90, 270)
speed = random.randint(20, 50) / 100
print("Turning", angle, "degrees, then speed", speed)
drivetrain.turn(angle)
else:
drivetrain.set_effort(speed, speed)
class LineTrack:
def __init__(self):
self.sensor = LineSensor()
self.drivetrain = DifferentialDrive.get_default_differential_drive()
self.base_effort = 0.4
self.kp = 0.5
self.turn_effort = 0.25
def track_to_intersection(self):
while not self.sensor.is_at_cross():
error = self.sensor.get_error()
correction = error * self.kp
self.drivetrain.arcade(self.base_effort, correction)
self.drivetrain.stop()
self.drivetrain.straight(8.75, 0.5)
def turn_clockwise(self):
self.drivetrain.turn(-60, self.turn_effort)
self.drivetrain.arcade(0, -self.turn_effort)
while not self.sensor.is_either_on_line():
pass
self.drivetrain.stop()
def turn_counterclockwise(self):
self.drivetrain.turn(60, self.turn_effort)
self.drivetrain.arcade(0, self.turn_effort)
while not self.sensor.is_either_on_line():
pass
self.drivetrain.stop()
tracker = LineTrack()
def test_follow():
while True:
error = sensor.get_error()
correction = error * tracker.kp
drivetrain.arcade(tracker.base_effort, correction)
time.sleep(0.01)
def test_sweep():
for i in range(500):
error = sensor.get_error()
correction = error * tracker.kp
if i % 50 == 0:
print(f"i={i} error={error:.2f} correction={correction:.2f}")
time.sleep(0.02)
def test_cross_moving():
while not sensor.is_at_cross():
drivetrain.set_effort(0.3, 0.3)
drivetrain.stop()
print("Cross!")
def test_track():
tracker.track_to_intersection()
print("Done!")
def test_reverse():
tracker.track_to_intersection()
drivetrain.turn(180)
tracker.track_to_intersection()
print("Done!")
def test_count():
for count in range(4):
tracker.track_to_intersection()
print("Cross #", count + 1)
drivetrain.turn(180)
print("Done!")
def turn_around():
tracker.turn_clockwise()
def test_turn():
tracker.track_to_intersection()
tracker.turn_clockwise()
def test_course():
tracker.track_to_intersection()
tracker.turn_clockwise()
tracker.track_to_intersection()
print("Done!")
def test_turn_around():
tracker.track_to_intersection()
turn_around()
tracker.track_to_intersection()
print("Done!")
def test_reversal():
tracker.track_to_intersection()
tracker.turn_clockwise()
tracker.track_to_intersection()
print("Done!")
def test_final_project():
print("Module 2 Final Project -- starting!")
for leg in range(4):
print("Leg", leg + 1, "-- following to the cross...")
tracker.track_to_intersection()
print("Cross detected! Reversing...")
tracker.turn_clockwise()
print("Done! 4 reversals complete.")
def test_first_intersection():
print("Driving to the first intersection...")
tracker.track_to_intersection()
print("Intersection reached!")
def test_arrive_and_turn():
tracker.track_to_intersection()
print("Turning right...")
tracker.turn_clockwise()
print("Now facing a new direction!")
def drive_intersections(count):
"""Arrive at count intersections in a row, printing
which one it is each time. Ends parked on the last one
with the sensors past it, ready to turn or to go on."""
for i in range(count):
print(" Intersection", i + 1, "of", count)
tracker.track_to_intersection()
def test_two_by_hand():
tracker.track_to_intersection()
tracker.track_to_intersection()
print("Parked on the second intersection")
def test_drive_run():
drive_intersections(3)
print("Done! Parked on intersection 3")
def test_l_shape():
drive_intersections(2)
tracker.turn_clockwise()
drive_intersections(2)
print("L-shape complete!")
def test_my_path():
drive_intersections(3)
tracker.turn_counterclockwise()
drive_intersections(1)
print("Path complete!")
SIDE = 2
def test_one_side():
drive_intersections(SIDE)
tracker.turn_clockwise()
print("One side done")
def test_square():
print("Module 3 Final Project -- Square Pattern!")
for side in range(4):
print("Side", side + 1, "of 4")
drive_intersections(SIDE)
print(" Turning right...")
tracker.turn_clockwise()
print("Square complete! Back at start.")
class Manhattan:
"""The Lesson 4-3 planner as a class.
It holds NO state: compute_path() is told the robot's position
every time. Manhattan is a class only so that it has the same
shape as the Module 5 and 6 planners (BFS, Dijkstra) -- then one
main program and one test harness work with any of them by
changing one name.
"""
def __init__(self):
pass
def compute_path(self, start, destination):
print("computing path from:", start, "to:", destination)
path = []
x, y = start
dest_x, dest_y = destination
while x < dest_x:
x = x + 1
path.append((x, y))
while x > dest_x:
x = x - 1
path.append((x, y))
while y < dest_y:
y = y + 1
path.append((x, y))
while y > dest_y:
y = y - 1
path.append((x, y))
return path
def add_blocked(self, intersection):
pass
def clear_blocked(self):
pass
def run_test(test_name, start, destination, expected):
"""Plan start -> destination and compare the result
with expected; print test_name with PASS or FAIL."""
actual = Manhattan().compute_path(start, destination)
if actual == expected:
print(test_name, "- PASS")
else:
print(test_name, "- FAIL")
print(" Expected:", expected)
print(" Actual: ", actual)
def test_planner():
run_test("Leg 1", (0, 0), (2, 3), [(1, 0), (2, 0), (2, 1), (2, 2), (2, 3)])
run_test("Leg 2", (2, 3), (1, 0), [(1, 3), (1, 2), (1, 1), (1, 0)])
run_test("Same column", (2, 0), (2, 3), [(2, 1), (2, 2), (2, 3)])
run_test("Same row", (0, 2), (3, 2), [(1, 2), (2, 2), (3, 2)])
run_test("Same spot", (1, 1), (1, 1), [])
HEADING_NAMES = ["N", "E", "S", "W"]
class Navigator:
def __init__(self, start, heading):
self.position = start
self.heading = heading
self.line_track = LineTrack()
def get_desired_heading(self, next_pos):
x, y = self.position
next_x, next_y = next_pos
if next_y > y: return 0
if next_x > x: return 1
if next_y < y: return 2
if next_x < x: return 3
raise ValueError(str(next_pos) + " is not one step from " + str(self.position))
def turn_to(self, desired_heading):
while self.heading != desired_heading:
self.line_track.turn_clockwise()
self.heading = (self.heading + 1) % 4
def drive_path(self, path):
for next_pos in path:
desired_heading = self.get_desired_heading(next_pos)
self.turn_to(desired_heading)
self.line_track.track_to_intersection()
self.position = next_pos
def navigate_with_detection(self, destination, rangefinder, obstacle_cm):
"""Drive from self.position (facing self.heading) to
destination, re-planning around every obstacle the
rangefinder finds; return (step_count, reroute_count)."""
step_count = 0
reroute_count = 0
arrived = False
while not arrived:
path = planner.compute_path(self.position, destination)
print(" Path from", self.position, ":", path)
if len(path) == 0:
print("No path to", destination, "!")
return step_count, reroute_count
rerouted = False
for next_pos in path:
desired_heading = self.get_desired_heading(next_pos)
self.turn_to(desired_heading)
distance = rangefinder.distance()
if distance < obstacle_cm:
planner.add_blocked(next_pos)
print(" OBSTACLE at", next_pos, "! Rerouting...")
reroute_count = reroute_count + 1
rerouted = True
break
else:
self.line_track.track_to_intersection()
self.position = next_pos
step_count = step_count + 1
if not rerouted:
arrived = True
return step_count, reroute_count
navigator = Navigator((0, 0), 0)
def test_headings():
nav = Navigator((1, 1), 0)
for next_pos in [(0, 1), (1, 2), (2, 1), (1, 0)]:
heading = nav.get_desired_heading(next_pos)
print("(1, 1) to", next_pos, ":", heading, HEADING_NAMES[heading])
def fake_track_to_intersection():
"""STUB for tracker.track_to_intersection(): print instead of
driving. Lesson 4-6 swaps in the real one."""
print("tracking to intersection")
def fake_turn_clockwise():
"""STUB for tracker.turn_clockwise(): print instead of
turning. Lesson 4-6 swaps in the real one."""
print("turning clockwise")
DELIVERY_STOPS = [(2, 3), (1, 0), (3, 1), (2, 2), (0, 0)]
def test_delivery():
for delivery_destination in DELIVERY_STOPS:
path = planner.compute_path(navigator.position, delivery_destination)
print(path)
navigator.drive_path(path)
print("new position:", navigator.position,
"heading:", HEADING_NAMES[navigator.heading])
ROWS = 4
COLS = 4
def build_graph(rows, cols, blocked):
"""Return the grid as a dictionary, leaving out every
intersection in blocked: not a key, not anyone's neighbor."""
graph = {}
for x in range(cols):
for y in range(rows):
if (x, y) in blocked:
continue
neighbors = []
for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
nx, ny = x + dx, y + dy
if 0 <= nx < cols and 0 <= ny < rows and (nx, ny) not in blocked:
neighbors.append((nx, ny))
graph[(x, y)] = neighbors
return graph
def test_grid_graph():
graph = build_graph(ROWS, COLS, planner.blocked)
print("Intersections:", len(graph))
print("Neighbors of (1, 1):", graph[(1, 1)])
print("Neighbors of (0, 0):", graph[(0, 0)])
def test_blocked_graph():
planner.add_blocked((1, 1))
graph = build_graph(ROWS, COLS, planner.blocked)
print("Blocked:", planner.blocked)
print("Number of intersections:", len(graph))
print("(1,1) in graph?", (1, 1) in graph)
print("Neighbors of (0,1):", graph[(0, 1)])
planner.add_blocked((1, 1))
print("Blocked after adding (1,1) again:", planner.blocked)
print("Path:", planner.compute_path((0, 0), (3, 3)))
class BFS:
def __init__(self, rows=4, cols=4):
self.rows = rows
self.cols = cols
self.blocked = []
def add_blocked(self, intersection):
if intersection not in self.blocked:
self.blocked.append(intersection)
def clear_blocked(self):
self.blocked = []
def compute_path(self, start, destination):
graph = build_graph(self.rows, self.cols, self.blocked)
if start not in graph or destination not in graph:
return []
previous = {start: None}
to_visit = [start]
while len(to_visit) > 0:
current = to_visit.pop(0)
if current == destination:
break
for neighbor in graph[current]:
if neighbor not in previous:
previous[neighbor] = current
to_visit.append(neighbor)
if destination not in previous:
return []
path = []
current = destination
while current != start:
path.append(current)
current = previous[current]
path.reverse()
return path
planner = BFS()
def bfs_rings(start):
"""Ring number of every intersection reachable from
start, as a dictionary -- the search with no break."""
graph = build_graph(ROWS, COLS, planner.blocked)
if start not in graph:
return {}
rings = {start: 0}
to_visit = [start]
while len(to_visit) > 0:
current = to_visit.pop(0)
for neighbor in graph[current]:
if neighbor not in rings:
rings[neighbor] = rings[current] + 1
to_visit.append(neighbor)
return rings
def print_grid(rows, cols, blocked, numbers):
"""Draw the grid the way it lies in front of you: the top
line is the far row, y = 0 is the bottom line. Each
intersection shows its entry in numbers, X if it is
blocked, . if it has no entry."""
for i in range(rows):
y = rows - 1 - i
line = " " + str(y) + " | "
for x in range(cols):
if (x, y) in blocked:
cell = "X"
elif (x, y) in numbers:
cell = numbers[(x, y)]
else:
cell = "."
line = line + "{:>3}".format(cell)
print(line)
print(" " + "-" * (3 * cols + 1))
footer = " "
for x in range(cols):
footer = footer + "{:>3}".format(x)
print(footer)
def test_bfs_open():
planner.clear_blocked()
for (start, dest, expect) in [((0, 0), (3, 3), 6),
((0, 0), (0, 3), 3),
((2, 2), (2, 2), 0)]:
path = planner.compute_path(start, dest)
print("===", start, "to", dest, "===")
print("Path:", path)
print("Steps:", len(path), "(expect", expect, ")")
def test_bfs_walls():
planner.clear_blocked()
planner.add_blocked((1, 0))
planner.add_blocked((1, 1))
print("=== wall across the grid, (0,0) to (3,0) ===")
print("Blocked:", planner.blocked)
path = planner.compute_path((0, 0), (3, 0))
print("Path:", path)
print("Steps:", len(path), "(expect 7 -- around the wall)")
planner.add_blocked((1, 2))
planner.add_blocked((1, 3))
print("=== destination walled off ===")
print("Blocked:", planner.blocked)
path = planner.compute_path((0, 0), (3, 0))
print("Path:", path)
print("Steps:", len(path), "(expect 0 -- no way through)")
def test_rings():
planner.clear_blocked()
planner.add_blocked((1, 0))
planner.add_blocked((1, 1))
print("=== Rings from (0,0) with (1,0) and (1,1) blocked ===")
print_grid(ROWS, COLS, planner.blocked, bfs_rings((0, 0)))
def test_two_planners():
manhattan = Manhattan()
bfs = BFS()
for p in [manhattan, bfs]:
p.add_blocked((1, 0))
p.add_blocked((1, 1))
print("Manhattan:", manhattan.compute_path((0, 0), (3, 0)))
print("BFS: ", bfs.compute_path((0, 0), (3, 0)))
HARNESS_BLOCKED = []
def block(planner, intersection):
"""Tell planner about intersection AND note it in
HARNESS_BLOCKED, so check_path() can see it."""
planner.add_blocked(intersection)
if intersection not in HARNESS_BLOCKED:
HARNESS_BLOCKED.append(intersection)
def clear(planner):
"""Tell planner to forget its blocks, and forget ours."""
planner.clear_blocked()
while len(HARNESS_BLOCKED) > 0:
HARNESS_BLOCKED.pop()
def manhattan_distance(a, b):
"""Steps between a and b with nothing in the way."""
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def is_neighbor(a, b):
"""True when a and b are one step apart."""
return manhattan_distance(a, b) == 1
def check_path(path, start, destination):
"""Return a list of everything wrong with path as a route
from start to destination; [] means it is good."""
problems = []
if len(path) == 0:
if start != destination:
problems.append("empty path, but the robot is not at the destination")
return problems
if not is_neighbor(start, path[0]):
problems.append("first step " + str(path[0]) + " is not next to the start " + str(start))
if path[-1] != destination:
problems.append("last step " + str(path[-1]) + " is not the destination " + str(destination))
for i in range(1, len(path)):
if not is_neighbor(path[i - 1], path[i]):
problems.append("step " + str(path[i]) + " is not next to " + str(path[i - 1]))
for step in path:
if step in HARNESS_BLOCKED:
problems.append("drives through blocked intersection " + str(step))
return problems
def run_path_test(test_name, planner, start, destination, expected_steps):
"""Ask planner for a path, check the five properties, print
PASS or FAIL with the path; return True on a pass."""
path = planner.compute_path(start, destination)
problems = check_path(path, start, destination)
if len(path) != expected_steps:
problems.append("expected " + str(expected_steps) + " steps, got " + str(len(path)))
if len(problems) == 0:
print(" PASS ", test_name, "", path)
return True
print(" FAIL ", test_name, "", path)
for problem in problems:
print(" -", problem)
return False
def run_no_path_test(test_name, planner, start, destination):
"""For a walled-off destination the only right answer is
[]; print PASS or FAIL and return True on a pass."""
path = planner.compute_path(start, destination)
if len(path) == 0:
print(" PASS ", test_name, "", path)
return True
print(" FAIL ", test_name, "", path)
print(" - there is no way through, so the path should be empty")
return False
def run_all_tests(name, planner):
"""The six cases, through whichever planner is handed in:
four on the open grid, the wall, and walled off."""
print("==========", name, "==========")
passed = 0
clear(planner)
if run_path_test("open (0, 0) to (3, 3)", planner, (0, 0), (3, 3), 6):
passed = passed + 1
if run_path_test("open (0, 0) to (0, 3)", planner, (0, 0), (0, 3), 3):
passed = passed + 1
if run_path_test("open (2, 1) to (0, 3)", planner, (2, 1), (0, 3), 4):
passed = passed + 1
if run_path_test("open (1, 1) to (1, 1)", planner, (1, 1), (1, 1), 0):
passed = passed + 1
block(planner, (1, 0))
block(planner, (1, 1))
if run_path_test("wall at (1,0),(1,1): (0,0) to (3,0)", planner, (0, 0), (3, 0), 7):
passed = passed + 1
block(planner, (1, 2))
block(planner, (1, 3))
if run_no_path_test("walled off: (0,0) to (3,0)", planner, (0, 0), (3, 0)):
passed = passed + 1
clear(planner)
print(name + ":", passed, "of 6 passed")
def navigate(planner, position, destination):
"""Plan with planner and print the path; return
destination, where the robot would be after driving it."""
path = planner.compute_path(position, destination)
print(" Path from", position, "to", destination, ":", path, " Steps:", len(path))
return destination
def test_harness():
run_all_tests("Manhattan", Manhattan())
run_all_tests("BFS", BFS())
def test_swap():
planner.clear_blocked()
planner.add_blocked((2, 0))
print("Manhattan:")
position = navigate(Manhattan(), (0, 0), (3, 3))
position = navigate(Manhattan(), position, (0, 0))
print("BFS:")
position = navigate(planner, (0, 0), (3, 3))
position = navigate(planner, position, (0, 0))
OBSTACLE_CM = 15
SIMULATED_OBSTACLES = [(2, 0), (2, 2)]
def simulate_rangefinder(next_pos):
"""Stand-in for rangefinder.distance(): 8.0 if next_pos is
in SIMULATED_OBSTACLES, otherwise 50.0."""
return 8.0 if next_pos in SIMULATED_OBSTACLES else 50.0
def test_rangefinder():
for i in range(50):
print("Distance:", round(rangefinder.distance(), 1), "cm")
time.sleep(0.1)
def test_detect_once():
destination = (3, 3)
path = planner.compute_path(navigator.position, destination)
print("Planned path:", path)
board.wait_for_button()
for next_pos in path:
desired_heading = navigator.get_desired_heading(next_pos)
navigator.turn_to(desired_heading)
distance = simulate_rangefinder(next_pos)
print("At", navigator.position, "facing", HEADING_NAMES[navigator.heading],
"- ahead:", distance, "cm")
if distance < OBSTACLE_CM:
planner.add_blocked(next_pos)
print("Obstacle at", next_pos, "! Rerouting...")
path = planner.compute_path(navigator.position, destination)
print("New path:", path)
break
else:
navigator.line_track.track_to_intersection()
navigator.position = next_pos
print("Drove to", navigator.position)
print("All known obstacles:", planner.blocked)
KNOWN_OBSTACLES = []
def test_navigate():
for intersection in KNOWN_OBSTACLES:
planner.add_blocked(intersection)
destination = (3, 3)
print("Planned path:", planner.compute_path(navigator.position, destination))
board.wait_for_button()
steps, reroutes = navigator.navigate_with_detection(
destination, rangefinder, OBSTACLE_CM
)
print("Arrived at:", navigator.position, "facing", HEADING_NAMES[navigator.heading])
print("Steps:", steps, " Reroutes:", reroutes)
print("All known obstacles:", planner.blocked)
CAPSTONE_DESTINATIONS = [(1, 3), (3, 3), (3, 0), (0, 0)]
def test_capstone_paper():
for intersection in KNOWN_OBSTACLES:
planner.add_blocked(intersection)
position = (0, 0)
for dest in CAPSTONE_DESTINATIONS:
path = planner.compute_path(position, dest)
print("To", dest, ":", path, " steps:", len(path))
position = dest
def test_capstone():
for intersection in KNOWN_OBSTACLES:
planner.add_blocked(intersection)
total_steps = 0
total_reroutes = 0
print("Planned path for leg 1:",
planner.compute_path(navigator.position, CAPSTONE_DESTINATIONS[0]))
board.wait_for_button()
for i in range(len(CAPSTONE_DESTINATIONS)):
dest = CAPSTONE_DESTINATIONS[i]
print("--- Leg", i + 1, ": Heading to", dest, "---")
steps, reroutes = navigator.navigate_with_detection(
dest, rangefinder, OBSTACLE_CM
)
total_steps = total_steps + steps
total_reroutes = total_reroutes + reroutes
print(" Arrived at", dest)
print("=== FINAL REPORT ===")
print("Total steps:", total_steps)
print("Total reroutes:", total_reroutes)
print("Obstacles discovered:", planner.blocked)
board.wait_for_button()