from XRPLib.reflectance import Reflectance
from XRPLib.differential_drive import DifferentialDrive
from XRPLib.board import Board
import time
import random
reflectance = Reflectance.get_default_reflectance()
drivetrain = DifferentialDrive.get_default_differential_drive()
board = Board.get_default_board()
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
planner = Manhattan()
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 = planner.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
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])
board.wait_for_button()