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.")
board.wait_for_button()