def main(): camera = Camera(1, True) while True: blocks, frame = camera.get_block_coords() position, angle, frame2 = camera.get_robot_position() nav = Navigate() if blocks and position: block_data = nav.calculate_distances_angles( blocks, position, angle) print(nav.add_block_to_rejects(block_data, position, angle)) cv2.imshow('frame', frame) cv2.imshow('frame2', frame2) k = cv2.waitKey(5) & 0xFF if k == 27: break
def main(): camera = Camera(1, True) while True: blocks, frame = camera.get_block_coords([91, 114]) position, angle, frame2 = camera.get_robot_position() nav = Navigate() #print(blocks) if blocks and position: block_data = nav.calculate_distances_angles(blocks, position, angle) best_block = nav.choose_next_block(block_data) #print("best") #print(block_data[best_block][3]) cv2.circle(frame2, (int(block_data[best_block][0]), int(block_data[best_block][1])), 5, (255,0,0), 3) print(nav.block_in_range(best_block, block_data, position, angle)) cv2.imshow('frame', frame) cv2.imshow('frame2', frame2) k = cv2.waitKey(5) & 0xFF if k == 27: break
def run(): arduino = Arduino_Connection(com="com19") # find right com channel #setup controller KP = 0.75 KI = 0 KD = 0.5 controller = PID(KP, KI, KD) camera = Camera(webcam_number=1, return_frame=True) position, robot_angle, frame = camera.get_robot_position() cv2.imshow('frame', frame) while 1: position, robot_angle, frame = camera.get_robot_position(robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) blocks, blocks_frame = camera.get_block_coords([95, 105]) cv2.imshow('blocks frame', blocks_frame) nav = Navigate() block_data = None if blocks and position: block_data = nav.calculate_distances_angles(blocks, position, robot_angle) best_block = nav.choose_next_block(block_data) #print("best") print(block_data[best_block][3]) cv2.circle(blocks_frame, (int(block_data[best_block][0]), int(block_data[best_block][1])), 5, (255,0,0), 3) cv2.imshow('frame', frame) if block_data and robot_angle: desired_angle = block_data[best_block][3] + robot_angle control(arduino, controller, robot_angle, desired_angle, debug=True) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break
def run(): arduino = Arduino_Connection(com="com19") # find right com channel #setup controller KP = 0.75 KI = 0.1 KD = 0.75 controller = PID(KP, KI, KD) camera = Camera(webcam_number=1, return_frame=True) position, robot_angle, frame = camera.get_robot_position() navigate = Navigate() pick_up_drive = pick_up() cv2.imshow('frame', frame) blocks = None while blocks == None: blocks, blocks_frame = camera.get_block_coords() #[95, 105]) block_data = navigate.calculate_distances_angles(blocks, position, robot_angle) navigate.reject_line(block_data) corner = False while not corner: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (500, 60), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, corner = navigate.go_to_point( position, robot_angle, [500, 60]) if not corner: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break top_line = False while not top_line: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (520, 130), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, top_line = navigate.go_to_point( position, robot_angle, [520, 130]) if not top_line: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break mid_line = False while not mid_line: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (520, 430), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, mid_line = navigate.go_to_point( position, robot_angle, [520, 280]) if not mid_line: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break bottom_line = False while not bottom_line: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (520, 430), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, bottom_line = navigate.go_to_point( position, robot_angle, [520, 430]) if not bottom_line: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break accepted_blocks = 0 rejected_blocks = 0 detect_reject = False while 1: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.imshow('blocks frame', blocks_frame) #block_data = None if blocks and position: block_data = navigate.calculate_distances_angles( blocks, position, robot_angle) best_block = navigate.choose_next_block(block_data) #print("best") #print(block_data[best_block][3]) cv2.circle(blocks_frame, (int( block_data[best_block][0]), int(block_data[best_block][1])), 5, (255, 0, 0), 3) cv2.imshow('frame', frame) for block in block_data: rejects = navigate.return_rejects() arrived = False for reject in rejects: if abs(block_data[block][0] - reject[0]) < 30 and abs( block_data[block][0] - reject[0]) < 30: arrived = True break else: arrived = False while not arrived: position, robot_angle, frame = camera.get_robot_position( robot_position_colour_bounds=np.array( [[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle( frame, (int(block_data[block][0]), int(block_data[block][1])), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, arrived = navigate.go_to_point( position, robot_angle, [int(block_data[block][0]), int(block_data[block][1])]) if not arrived: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break top_green = False while not top_green: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (50, 200), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, top_green = navigate.go_to_point( position, robot_angle, [50, 200]) if not top_green: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break green = False while not green: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (50, 250), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, green = navigate.go_to_point( position, robot_angle, [50, 250]) if not green: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break arduino.open_back_gate() end = False while not end: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (50, 500), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, end = navigate.go_to_point( position, robot_angle, [50, 500]) if not end: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break arduino.drive(0, 0) arduino.close_back_gate() break time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break
def run(): arduino = Arduino_Connection(com="com19") # find right com channel #setup controller KP = 0.75 KI = 0 KD = 0.75 controller = PID(KP, KI, KD) camera = Camera(webcam_number=1, return_frame=True) position, robot_angle, frame = camera.get_robot_position() navigate = Navigate() pick_up_drive = pick_up() cv2.imshow('frame', frame) blocks = None while blocks == None: blocks, blocks_frame = camera.get_block_coords() #[95, 105]) block_data = navigate.calculate_distances_angles(blocks, position, robot_angle) navigate.reject_line(block_data) position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.imshow('blocks frame', blocks_frame) #block_data = None if blocks and position: block_data = navigate.calculate_distances_angles( blocks, position, robot_angle) best_block = navigate.choose_next_block(block_data) #print("best") #print(block_data[best_block][3]) cv2.circle( blocks_frame, (int(block_data[best_block][0]), int(block_data[best_block][1])), 5, (255, 0, 0), 3) cv2.imshow('frame', frame) """Go to all blocks that are randomly scattered""" for block in block_data: rejects = navigate.return_rejects() arrived = False for reject in rejects: if abs(block_data[block][0] - reject[0]) < 30 and abs( block_data[block][0] - reject[0]) < 30: arrived = True break else: arrived = False while not arrived: position, robot_angle, frame = camera.get_robot_position( robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (int(block_data[block][0]), int(block_data[block][1])), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position and robot_angle: desired_angle, arrived = navigate.go_to_point( position, robot_angle, [int(block_data[block][0]), int(block_data[block][1])]) if not arrived: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break """Drop off blocks""" top_green = False while not top_green: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (75, 200), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, top_green = navigate.go_to_point( position, robot_angle, [50, 200]) if not top_green: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break for i in range(60): position, robot_angle, frame = camera.get_robot_position() cv2.imshow('frame', frame) if robot_angle: control(arduino, controller, robot_angle, -math.pi) time.sleep(0.1) arduino.turn_out_left() time.sleep(2) arduino.open_back_gate() green = False while not green: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (70, 400), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, green = navigate.go_to_point( position, robot_angle, [70, 400]) if not green: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break while position[1] < 370: position, robot_angle, frame = camera.get_robot_position() cv2.imshow('frame', frame) while position == None: position, robot_angle, frame = camera.get_robot_position() time.sleep(0.1) """Set to True to try get blocks on line or False to ignore them""" line = False if line: arduino.close_back_gate() corner = False while not corner: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (500, 90), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, corner = navigate.go_to_point( position, robot_angle, [500, 90]) if not corner: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break for i in range(50): position, robot_angle, frame = camera.get_robot_position() cv2.imshow('frame', frame) if robot_angle: control(arduino, controller, robot_angle, 0) time.sleep(0.1) arduino.turn_out_right() while position[1] < 430: position, robot_angle, frame = camera.get_robot_position() cv2.imshow('frame', frame) while position == None: position, robot_angle, frame = camera.get_robot_position() control(arduino, controller, robot_angle, 1.6) time.sleep(0.1) top_green = False while not top_green: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (75, 200), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, top_green = navigate.go_to_point( position, robot_angle, [50, 200]) if not top_green: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break for i in range(60): position, robot_angle, frame = camera.get_robot_position() cv2.imshow('frame', frame) if robot_angle: control(arduino, controller, robot_angle, -math.pi) time.sleep(0.1) arduino.turn_out_left() time.sleep(2) arduino.open_back_gate() while position[1] < 370: position, robot_angle, frame = camera.get_robot_position() cv2.imshow('frame', frame) while position == None: position, robot_angle, frame = camera.get_robot_position() time.sleep(0.1) end = False while not end: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (50, 500), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, end = navigate.go_to_point(position, robot_angle, [50, 500]) if not end: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break arduino.drive(0, 0) arduino.close_back_gate()
def run(): arduino = Arduino_Connection(com="com19") # find right com channel #setup controller KP = 0.75 KI = 0 KD = 0.75 controller = PID(KP, KI, KD) camera = Camera(webcam_number=1, return_frame=True) position, robot_angle, frame = camera.get_robot_position() navigate = Navigate() pick_up_drive = pick_up() cv2.imshow('frame', frame) blocks, blocks_frame = camera.get_block_coords() #[95, 105]) block_data = navigate.calculate_distances_angles(blocks, position, robot_angle) nav.reject_line(block_data) corner = False while not corner: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (520, 60), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, corner = navigate.go_to_point( position, robot_angle, [520, 60]) if not corner: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break top_line = False while not top_line: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (530, 130), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, top_line = navigate.go_to_point( position, robot_angle, [530, 130]) if not top_line: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break mid_line = False while not mid_line: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (530, 430), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, mid_line = navigate.go_to_point( position, robot_angle, [530, 280]) if not mid_line: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break bottom_line = False while not bottom_line: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.circle(frame, (530, 430), 3, (255, 0, 0), 2) cv2.imshow('frame', frame) #print(corner) if position: desired_angle, bottom_line = navigate.go_to_point( position, robot_angle, [530, 430]) if not bottom_line: #print(robot_angle) control(arduino, controller, robot_angle, desired_angle) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break accepted_blocks = 0 rejected_blocks = 0 detect_reject = False while 1: position, robot_angle, frame = camera.get_robot_position( ) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]])) cv2.imshow('blocks frame', blocks_frame) block_data = None if blocks and position: block_data = navigate.calculate_distances_angles( blocks, position, robot_angle) best_block = navigate.choose_next_block(block_data) #print("best") #print(block_data[best_block][3]) cv2.circle(blocks_frame, (int( block_data[best_block][0]), int(block_data[best_block][1])), 5, (255, 0, 0), 3) cv2.imshow('frame', frame) #if navigate.block_in_range(block_data, position, robot_angle): # pick_up_drive.drive_to_collect() if block_data and robot_angle: desired_angle = block_data[best_block][3] + robot_angle control(arduino, controller, robot_angle, desired_angle) state = arduino.get_block_state() if state != Block_States.NO_BLOCK: #state = arduino.get_block_state() #print("state: ") #print(state) if state == 2: accepted_blocks += 1 print(state) detect_reject = True if state == 3: rejected_blocks += 1 print(state) detect_reject = True if detect_reject: rejected = navigate.add_block_to_rejects(block_data, position, robot_angle) if rejected == True: detect_reject = False if accepted_blocks >= 5: while True: relative_angle, arrived = navigate.go_to_point( position, robot_angle, [50, 300]) if not arrived: desired_angle = relative_angle + robot_angle control(arduino, controller, robot_angle, desired_angle, debug=True) for reject in navigate.reject_blocks: print(reject) time.sleep(0.1) k = cv2.waitKey(5) & 0xFF if k == 27: break