world = load_map("assets/test_plattegrond.txt")
robot = None
# Verbind met de robot Kies tussen echte of gesimuleerde robot
if SIMULATION:
robot = Robot()
else:
robot = HistogramFilter(world, 6, 4, start_orientation)
robot.draw()
robot.connect_color()
markov = Markov(world, start_orientation, "rddesmit")
found = False
while not found:
location = markov.current_estimate()
destination = (1, 1)
# drive
robot.drive(0.10)
markov.move()
# rotate
direction = Orientation.calculate_direction((location[1], location[2]), destination)
while direction != markov._orientation:
rotate_direction = Orientation.rotate(markov._orientation, direction)
if rotate_direction == Orientation.LEFT:
markov.rotate_left()
robot.turn_left()
time.sleep(3)
