class Robot: def __init__(self, mqtt_client, logger): # Create Planet and planet_name variable self.planet = Planet() self.planet_name = None # Setup Sensors, Motors and Odometry self.rm = ev3.LargeMotor("outB") self.lm = ev3.LargeMotor("outC") self.sound = ev3.Sound() self.odometry = Odometry(self.lm, self.rm) self.motors = Motors(self.odometry) self.cs = ColorSensor(self.motors) self.us = Ultrasonic() # Setup Communication self.communication = Communication(mqtt_client, logger, self) # Create variable to write to from communication self.start_location = None self.end_location = None self.path_choice = None self.running = True def run(self): counter = 0 bottle_detected = False previous_l, previous_r, previous_b = 0, 0, 350 start_message = """ ################################## # # # RoboLab Praktikum 2020 # # Exploration by Paula # # ❰❱ with ❤ by # # Eric, Marc, Nico # # # ################################## """ # Print start screen print(start_message) # Calibrate colors for better color accuracy self.cs.calibrate_colors() # Wait until we want to start print("» Press Button to start") sensors.button_pressed() start_time = time.time() print(time.strftime("Starttime: %H:%M:%S", time.localtime())) print("Lets start to explore...") # Start fancy features feature_threads = features.start_features(self) # Main robot loop while self.running: # Test if robot detect an obstacle if self.us.get_distance() < 15: # Rotate robot to drive back, and save that an obstacle was detected self.sound.play("/home/robot/src/assets/found.wav") print("Obstacle detected") self.cs.rotate_to_path(180) bottle_detected = True continue # Test if robot reached node if self.cs.get_node() in ["blue", "red"] and counter == 0: counter += 1 # Drive to center of node to better scanning self.motors.drive_in_center_of_node(100, 2) # Check if robot reached first node if self.planet_name is None: # Tell mothership to send planet information (planet_name and start coordinates) self.communication.send_ready() # Wait until message is received while self.planet_name is None: pass # Reset odometry of last path, not used for first path self.odometry.reset_list() # Current robot orientation forward_dir = self.start_location[1] # End-position and direction of incoming path self.end_location = (self.start_location[0], (forward_dir + 180) % 360) # Save path as blocked for better path calculation self.planet.add_path(self.end_location, self.end_location, -1) else: # Test if path is known if self.start_location[0] in self.planet.planet_dict \ and self.start_location[1] in self.planet.planet_dict[self.start_location[0]]: # Get end node from planet dictionary ((x, y), send_dir, _) = self.planet.planet_dict[self.start_location[0]][self.start_location[1]] # Reset odometry of last path, not used for first path self.odometry.reset_list() else: # Calculate postion and direction from odometry x, y, forward_dir = self.odometry.calculate_path( self.start_location[1], bottle_detected, self.start_location[0][0], self.start_location[0][1]) # Direction facing back, used for path-message send_dir = (forward_dir + 180) % 360 # Send path to mothership get real position and direction self.communication.send_path(self.start_location, ((x, y), send_dir), bottle_detected) # Wait until message is received while self.end_location is None: pass # Position of current node current_pos = self.end_location[0] # Direction of robot forward_dir = (self.end_location[1] + 180) % 360 # Test if robot already scanned node if current_pos in self.planet.scanned_nodes: # If scanned, wait if mothership sends information like target and pathUnveiled time.sleep(2) else: # Scan node for posible directions possible_dirs = self.cs.analyze(forward_dir) # Save direction into unexlored edges if they aren't in the planet dictionary for d in possible_dirs: if current_pos not in self.planet.planet_dict \ or d not in self.planet.planet_dict[current_pos].keys(): self.planet.add_unexplored_edge(current_pos, d) # Mark node as scanned self.planet.scanned_nodes.append(current_pos) # Test if robot reached the target if current_pos == self.planet.target: # Send targetReached to mothership self.communication.send_target_reached("Target reached!") # Wait until confirmation while self.running: pass self.sound.play("/home/robot/src/assets/smb_stage_clear.wav") continue # Calculate next direction based on planet information and posible directions next_dir = self.choose_dir() # If no direction is found: Exloration completed if next_dir == -1: # Send explorationCompleted to mothership self.communication.send_exploration_completed("Exloration completed!") # Wait until confirmation while self.running: pass self.sound.play("/home/robot/src/assets/metroid.wav") continue # Save new starting postion for next path self.start_location = (self.end_location[0], next_dir) # Rotate to new path self.cs.select_new_path(forward_dir, next_dir) # Reset variables self.end_location = None bottle_detected = False self.odometry.reset_position() else: # Follow line and save last data for next tick previous_l, previous_r, previous_b = self.motors.follow_line(self.cs, previous_l, previous_r, previous_b) # new (better solution?) -> multiple times calles -> ticks_previous needed counter = 0 print(time.strftime("Endtime: %H:%M:%S", time.localtime())) delta = (time.time() - start_time) / 60 print("Timedelta: %.2f min" % delta) for thread in feature_threads: thread.join() def choose_dir(self): # Next direction, -1 = no direction found choice = -1 # Test if target is set if self.planet.target is not None: # Calculate shortest path to target shortest = self.planet.shortest_path(self.end_location[0], self.planet.target) # Test if target is reachable if shortest is not None: # Select first direction to get to target choice = shortest[0][1] # Test if no direction is set (no target or target not reachable) if choice == -1: # Nodes which need be explored incomplete_nodes = [] # Shortest distance to unexplored node distance = math.inf # Nearest node nearest = None # Collect all node, which have open paths for node in self.planet.unexplored_edges.keys(): incomplete_nodes.append(node) # Collect all node, which aren't scanned for node in self.planet.get_nodes(): if node not in self.planet.scanned_nodes: incomplete_nodes.append(node) # Test if all nodes are explored if len(incomplete_nodes) == 0: return -1 # Calculate nearest open node for node in incomplete_nodes: cur = self.planet.shortest_distance(self.end_location[0], node) if cur < distance: distance = cur nearest = node # Test if no open node is reachable (explorationCompleted) if distance == math.inf: return -1 # Test if open node is reached, choose first open path elif distance == 0: choice = self.planet.unexplored_edges[nearest][0] # Otherwise drive to open node (select first direction to get to open node) else: choice = self.planet.shortest_next_dir(self.end_location[0], nearest) # Send pathSelect to mothership self.communication.send_path_select((self.end_location[0], choice)) # Wait until message receive or 3sec (answer optional) start_time = time.time() while self.path_choice is None and time.time() - start_time < 3.0: pass # Test if mothership overwrites direction, use forced direction if self.path_choice is not None: choice = self.path_choice print("Next direction: %s" % Direction(choice)) # Reset Variable self.path_choice = None # Play sound self.sound.play("/home/robot/src/assets/codecover.wav") return choice