robot.py 文件源码

def Main(trigger):
    print("   _____       _____ _  _____         ____  ")
    print("  / ____|     |  __ (_)/ ____|       |___ \ ")
    print(" | |  __  ___ | |__) || |  __  ___     __) |")
    print(" | | |_ |/ _ \|  ___/ | | |_ |/ _ \   |__ < ")
    print(" | |__| | (_) | |   | | |__| | (_) |  ___) |")
    print("  \_____|\___/|_|   |_|\_____|\___/  |____/ ")
    print("                                            ")

    print("Let your GoPiGo3 move around and avoid any obstacles.")
    print("Pay attention to how your GoPiGo3 moves around.")
    print("Avoid sharp corners / edges as the algorithm wasn't made for advanced stuff.")

    # Event object for letting one thread
    # control the other thread's flow control
    put_on_hold = threading.Event()
    # used for synchronizing the threads
    simultaneous_launcher = threading.Barrier(2)
    # for exchanging messages between threads
    sensor_queue = queue.Queue()

    print("\nWaiting threads to fire up")

    path_finder = threading.Thread(target=obstacleFinder, args=(trigger, put_on_hold, simultaneous_launcher, sensor_queue))
    controller = threading.Thread(target=robotController, args=(trigger, put_on_hold, simultaneous_launcher, sensor_queue))

    # start the threads
    path_finder.start()
    controller.start()

    # wait for the user to press CTRL-C
    # or to have an error while firing up the threads
    while not trigger.is_set() and not simultaneous_launcher.broken:
        sleep(0.001)
    # if an error was encountered
    if simultaneous_launcher.broken:
        # then exit the script
        sys.exit(1)

    # otherwise, just wait for the threads to finish
    path_finder.join()
    controller.join()

    # and then exit successfully
    sys.exit(0)