class Controller: def __init__(self): self.np_image = None self.move_robot = MoveRobot(UR_IP) self.vision = Vision() self.aruco = Calibration() print("[I] Controller running") def main_flow(self, colour_part_id): self.get_image() self.vision.detect_object() z_offset = 0 self.pick_and_place_part(PartEnum.BACKCOVER.value, z_offset) z_offset = 20 self.pick_and_place_part(PartEnum.PCB.value, z_offset) z_offset = 30 self.pick_and_place_part(PartEnum.FUSE.value, z_offset, fuse_id=0) self.pick_and_place_part(PartEnum.FUSE.value, z_offset, fuse_id=1) z_offset = 30 self.pick_and_place_part(colour_part_id, z_offset) self.move_robot.move_out_of_view() def pick_and_place_part(self, part_id, z_offset, fuse_id=-1): new_part_id, x, y, orientation, grip_width = self.get_part_location(part_id, fuse_index=fuse_id) while x is None: print("[W]: Could not find required part in image, please move the part and try again. Part: ", part_id_to_name(part_id)) input("Press Enter to continue...") self.get_image() self.vision.detect_object() new_part_id, x, y, orientation, grip_width = self.get_part_location(part_id) # print("[D]: Position: ", position, " orientation = ", orientation) self.move_and_grip(x, y, orientation, new_part_id, grip_width) self.move_robot.align() is_facing_right = False if not part_id == PartEnum.FUSE.value: if not part_id == PartEnum.PCB.value: self.move_robot.move_to_camera(is_pcb=False) else: self.move_robot.move_to_camera(is_pcb=True) np_image = self.vision.capture_image() is_facing_right = self.vision.is_facing_right(np_image) if is_facing_right: if part_id == PartEnum.PCB.value: self.move_robot.assemble(rotated=True, fuse_id=fuse_id) else: self.move_robot.assemble(rotated=False, fuse_id=fuse_id) else: if part_id == PartEnum.PCB.value: self.move_robot.assemble(rotated=False, fuse_id=fuse_id) else: self.move_robot.assemble(rotated=True, fuse_id=fuse_id) def move_and_grip(self, x, y, orientation, part_id, grip_width): print("[I] Moving arm") self.move_robot.grip(x, y, orientation, part_id, grip_width) def get_part_location(self, part_id, fuse_index=-1): class_names = convert_from_part_id(part_id) new_part_id, x, y, orientation, grip_width = self.vision.find_parts(class_names, fuse_index) if x == -1 and y == -1: return None, None, None, None, None x, y, _ = self.aruco.calibrate(self.np_image, x, y) if part_id == PartEnum.FUSE.value: fuse_in_restricted_area = self.fuse_area_check(y) while fuse_in_restricted_area: print("[W]: Fuse found in restricted area, y =", y, " please move the fuse closer to the robot") input("Press Enter to continue...") self.get_image() self.vision.detect_object() new_part_id, x, y, orientation, grip_width = self.vision.find_parts(class_names) # dont use fuse index here x, y, _ = self.aruco.calibrate(self.np_image, x, y) fuse_in_restricted_area = self.fuse_area_check(y) return new_part_id, x, y, orientation, grip_width def fuse_area_check(self, fuse_y): if fuse_y < -500: return True else: return False def parts_to_flip_contains(self, parts_to_flip, part_id): filtered_array = [part for part in parts_to_flip if part[0] == part_id] return True if len(filtered_array) > 0 else False def get_image(self): self.move_robot.move_out_of_view() self.np_image = self.vision.capture_image() def choose_action(self): print("Please write a command (write 'help' for a list of commands):") command = input() if command == "help": print( "Possible commands are: \nblack: assemble phone with black cover \nwhite: assemble phone with white cover \n" + "blue: assemble phone with blue cover \nzero: put the robot in zero position \nquit: close the program") elif command == "black": self.main_flow(PartEnum.BLACKCOVER.value) elif command == "white": self.main_flow(PartEnum.WHITECOVER.value) elif command == "blue": self.main_flow(PartEnum.BLUECOVER.value) elif command == "zero": self.move_robot.move_out_of_view() elif command == "quit": return True else: print("Invalid command, please try again") return False