class Driver(): # Wait .05 sec between loops # This waits after the loop is run. This will not subtract the time it took to run loop() from the total wait time. def __init__(self, webserver_queue=None, looprate=0.2): self.arduino = Arduino() self.navigation = Navigation(self.arduino) self.state = State.COLLECT_spin_and_search_cone # Define constants self.looprate = looprate # Variables updated from webserver self.webserver_queue = webserver_queue self.mode = "auto" self.manual_direction = "stop" # Variables used by states self.start_time = None self.ready_to_deliver = False def start(self): self.stop = False s = sched.scheduler(time.time, self.arduino.board.sleep) s.enter(0, 1, self.loop, (s,)) s.run() def stop_on_next_loop(self): self.stop = True def loop(self, sc): # Read webserver queue for new messages while((self.webserver_queue != None) and (len(self.webserver_queue) > 0)): self.process_message(self.webserver_queue.popleft()) # If stopped, just loop if (self.stop): sc.enter(self.looprate, 1, self.loop, (sc,)) return if (self.mode == "auto"): # ---- Collect Spin and Search Cone ---- if (self.state == State.COLLECT_spin_and_search_cone): if (self.start_time == None): self.start_time = time.time() if (time.time() - self.start_time >= constants.SPIN_TIME): self.start_time = None self.navigation.stop() self.change_state(State.COLLECT_wander_and_search_cone) # TODO: Use signatures with pixy else: status = self.navigation.spin_and_search_cone() if (status == "CONE_FOUND"): self.start_time = None self.change_state(State.COLLECT_approach_cone) # ---- Collect Wander and Search ---- elif (self.state == State.COLLECT_wander_and_search_cone): if (self.start_time == None): self.start_time = time.time() if (time.time() - self.start_time >= constants.WANDER_TIME): self.start_time = None self.navigation.stop() self.change_state(State.COLLECT_spin_and_search_cone) # TODO: Use signatures with pixy else: status = self.navigation.wander_and_search_cone() if (status == "CONE_FOUND"): self.start_time = None self.change_state(State.COLLECT_approach_cone) # ---- Collect Approach Cone ---- elif (self.state == State.COLLECT_approach_cone): status = self.navigation.approach_cone() if (status == "LOST_CONE"): self.change_state(State.COLLECT_wander_and_search_cone) elif (status == "CONE_IN_RANGE"): self.change_state(State.COLLECT_acquire_cone) # ---- Collect Acquire Cone ---- elif (self.state == State.COLLECT_acquire_cone): self.arduino.close_claw() ping = self.arduino.get_ping() if (ping <= constants.PING_CONE_THRESHOLD and ping != 0): if (self.ready_to_deliver == True): self.change_state(State.DELIVER_spin_and_search_target) self.ready_to_deliver = False else: print("Waiting for inter-bot command to deliver...") else: self.change_state(State.COLLECT_open_claw) # ---- Collect Open Claw ---- elif (self.state == State.COLLECT_open_claw): self.arduino.open_claw() self.change_state(State.COLLECT_approach_cone) # ---- Deliver Spin and Search Target ---- elif (self.state == State.DELIVER_spin_and_search_target): if (self.start_time == None): self.start_time = time.time() if (time.time() - self.start_time >= constants.SPIN_TIME): self.start_time = None self.navigation.stop() self.change_state(State.DELIVER_wander_and_search_target) # TODO: Use signatures with pixy else: status = self.navigation.spin_and_search_target() if (status == "TARGET_FOUND"): self.start_time = None self.change_state(State.DELIVER_approach_target) # ---- Deliver Wander and Search Target ---- elif (self.state == State.DELIVER_wander_and_search_target): if (self.start_time == None): self.start_time = time.time() if (time.time() - self.start_time >= constants.WANDER_TIME): self.start_time = None self.navigation.stop() self.change_state(State.DELIVER_spin_and_search_target) # TODO: Use signatures with pixy else: status = self.navigation.wander_and_search_target() if (status == "TARGET_FOUND"): self.start_time = None self.change_state(State.DELIVER_approach_target) # ---- Deliver Approach Target ---- elif (self.state == State.DELIVER_approach_target): status = self.navigation.approach_target() if (status == "LOST_TARGET"): self.change_state(State.DELIVER_wander_and_search_target) elif (status == "TARGET_IN_RANGE"): self.change_state(State.DELIVER_verify_target) # ---- Deliver Verify Target ---- elif (self.state == State.DELIVER_verify_target): self.change_state(State.DELIVER_release_cone) # ---- Deliver Release Cone ---- elif (self.state == State.DELIVER_release_cone): self.arduino.open_claw() self.arduino.board.sleep(1) self.navigation.reverse() self.arduino.board.sleep(2) self.navigation.spin_clockwise() self.arduino.board.sleep(3) print("Starting over...") self.change_state(State.COLLECT_spin_and_search_cone) try: endpoint = "http://" + constants.GROUP10_IP + constants.GROUP10_ENDPOINT_READY print("Hitting endpoint: " + endpoint) # requests.get(endpoint, timeout=0.001) urlopen(endpoint) except: print("Failed to hit Group10 Endpoint, trying again...") print("Just kidding, I'm giving up!") elif (self.mode == "manual"): # print("In manual mode") if (self.manual_direction == "stop"): (left_motor, right_motor) = (0.0, 0.0) elif (self.manual_direction == "forward"): (left_motor, right_motor) = (0.2, 0.2) elif (self.manual_direction == "backward"): (left_motor, right_motor) = (-0.2, -0.2) elif (self.manual_direction == "right"): (left_motor, right_motor) = (0.2, 0.0) elif (self.manual_direction == "left"): (left_motor, right_motor) = (0.0, 0.2) print("L: " + str(left_motor) + " R: " + str(right_motor)) self.arduino.set_motors(left_motor, right_motor) elif (self.mode == "kill"): self.shutdown() sys.exit(0) # Loop again after delay sc.enter(self.looprate, 1, self.loop, (sc,)) def change_state(self, new_state): print("[State Change] " + str(self.state) + " -> " + str(new_state)) self.state = new_state def process_message(self, message): if (message == "stop"): self.stop_on_next_loop() elif (message == "start"): self.start() elif (message == "print"): print("Print!") # Modes elif (message == "auto"): self.mode = "auto" elif (message == "manual"): self.mode = "manual" elif (message == "kill"): self.mode = "kill" # Inter-Bot commands elif (message == "bot_ready_to_deliver"): print("Received inter-bot communication: ready_to_deliver") self.ready_to_deliver = True # Manual Directions elif (message == "manual_forward"): self.manual_direction = "forward" elif (message == "manual_backward"): self.manual_direction = "backward" elif (message == "manual_right"): self.manual_direction = "right" elif (message == "manual_left"): self.manual_direction = "left" elif (message == "manual_stop"): self.manual_direction = "stop" elif (message == "manual_claw_open"): self.arduino.open_claw() elif (message == "manual_claw_close"): self.arduino.close_claw() def shutdown(self, signal=None, frame=None): self.arduino.shutdown()