def astest():
t = GPS(map_width=100,
map_length=100,
resolution=10,
start_x=50,
start_y=0)
target = (50, 20)
obstacles = scanner.mapping_scan()
print(obstacles)
#obstacles[:,0] *= -1
#print(obstacles)
# populate map with obstacles
for obst in obstacles:
picar_orientation = 0
# actual orientation = picar_orientation + obstacle_scan angle
orientation = obst[0] + picar_orientation
t.add_relative_obstacle(orientation=obst[0], distance=obst[1])
t.save_grid('maps/1object_scan_result.out')
instructions = t.set_navigation_goal(target)
# while not at target
while (len(instructions) > 0):
# convert instructions to polar
step = instructions.pop()
print(step)
def atestempty():
grid = np.zeros([20, 20], dtype=int)
t = GPS()
t.load_grid(grid, start_x=10, start_y=0)
instructions = t.set_navigation_goal((19, 10))
# while not at target
while (len(instructions) > 0):
# convert instructions to polar
step = instructions.pop()
print(step)
def drive_target(nav: GPS, target: tuple):
car_theta = 0
curr_distance = 0
picar = Car()
at_destination = False
while (not at_destination):
# scan for obstacles
obstacles = scanner.mapping_scan()
print(obstacles)
for obst in obstacles:
abs_orient = obst[0] + picar.orientation
nav.add_relative_obstacle(orientation=abs_orient, distance=obst[1])
instructions = nav.set_navigation_goal(target)
at_destination = drive_instructions(picar, nav, instructions)
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)
