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)