def drive_instructions(picar: Car, nav: GPS, instructions: deque):
# while not at target
while (len(instructions) > 0):
# convert instructions to polar
step = instructions.popleft()
print("directions: ", step)
direction = step[0] - picar.orientation
direction = (direction + 180) % 360 - 180
#print("turning angle", direction)
driven = 0
# change direction if needed
if direction > 0:
picar.turn_right(direction)
elif direction < 0:
picar.turn_left(abs(direction))
if step[1] >= 0:
driven = picar.drive_forward(distance=step[1])
else:
driven = picar.drive_backward(distance=abs(step[1]))
nav.update_postion(distance=int(driven), orientation=picar.orientation)
print("curr position", nav.position)
# if blocked rerout
if driven < step[1]:
return False
return True
class SimplePiCar(Car):
def __init__(self):
super().__init__()
self.nav = GPS(map_width = 100, map_length = 100, resolution = 10, start_x = 50, start_y = 0)
self.obstacle_queue = Queue()
self.cam = detect.TrafficCam()
def drive_step(self, distance: int, power: int = 2):
self.trip_meter.reset()
while(self.cam.detect_traffic() == True):
fc.stop()
print("Obstacle detected")
time.sleep(2)
while(self.trip_meter.distance < distance):
fc.forward(2)
fc.stop()
print(self.trip_meter.distance, distance)
# drives towards a target
def drive_target(self, target: tuple):
at_destination = False
while(target != self.nav.position):
self.nav.clear_grid()
# scan for obstacles
obstacles = scanner.mapping_scan()
print(self.orientation)
print(obstacles)
for obst in obstacles:
abs_orient = obst[0] + self.orientation
self.nav.add_relative_obstacle(orientation = abs_orient, distance = obst[1])
instructions = self.nav.set_navigation_goal(target)
if len(instructions) == 0:
print("unable to find path")
return
# take the first 3 instructions
print(target, self.nav.position)
self.execute_instructions(instructions)
print("arrived at destination")
# drive according to instructions until blocked or finished
def execute_instructions(self, instructions:deque):
# while not at target
steps_taken = 0
while(len(instructions) > 0 and steps_taken< 3):
# convert instructions to polar
step = instructions.popleft()
print("directions: ", step)
# calculate the shortest angle to turn
direction = step[0] - self.orientation
direction = (direction + 180) % 360 - 180
#print("turning angle", direction)
# change direction if needed
if direction > 0:
self.turn_right(direction)
elif direction < 0:
self.turn_left(abs(direction))
steps_taken += 1
self.drive_step(distance = step[1])
# update position
self.nav.update_postion(step[1], self.orientation)
