def hit_flare(cur_pos): fwd_count = 25 # around 5 sec bwd_count = 15 # around 3 sec ok_to_go = False while True: img_front = cam_front.read() img_down = cam_down.read() coords_front = gesture_detection.get_coord_from_detection(img_front) cur_pos[0], cur_pos[1], cur_pos[2] = localizer.get_pos(img_down, cur_pos[0], cur_pos[1], cur_pos[2]) x, y = coords_front[0][0], coords_front[0][1] if x < 700: movement.turn_right() continue elif x > 800: movement.turn_left() continue elif coords_front[3] >= 200: # change this after testing ok_to_go = True if ok_to_go is True and fwd_count is not 0: movement.move_fwd() fwd_count -= 1 elif bwd_count is not 0: movement.move_bwd() bwd_count -= 1 else: img_down = cam_down.read() return localizer.get_pos(img_down, cur_pos[0], cur_pos[1], cur_pos[2])
import gesture_detection import time import cv2 from camera_module import camera_thread #,camera_thread_1 camera_thread = camera_thread() camera_thread.start() #camera_thread_1 = camera_thread_1() #camera_thread_1.start() while True: t1 = time.time() img = camera_thread.read() #img_1 = camera_thread_1.read() #cv2.imshow('webcam',img_1) #cv2.waitKey(1) coords = gesture_detection.get_coord_from_detection(img, "handphone") #coords_1 = gesture_detection.get_coord_from_detection(img_1,"webcam") #print (coords) t2 = time.time() print(t2 - t1)
#dxl_io.set_goal_position({found_ids[0]: 0,found_ids[2]: 0,found_ids[1]: 0}) for i in range(1000): dxl_io.set_moving_speed({found_ids[0]:150}) dxl_io.set_goal_position({found_ids[0]: 0}) dxl_io.set_moving_speed({found_ids[2]:150}) dxl_io.set_goal_position({found_ids[2]: 0}) dxl_io.set_moving_speed({found_ids[1]:150}) dxl_io.set_goal_position({found_ids[1]: 0}) time.sleep(1) while True: t1=time.time() img = camera_thread.read() img_height,img_width,_=img.shape coords = gesture_detection.get_coord_from_detection(img) if coords: for coord in coords: x_hand,y_hand,w_hand,h_hand,z_hand,cat_hand = coord arm.ee = [(x_hand-320)/3200, (y_hand-240)/2400,z_hand/100] arm_deg=np.round(np.rad2deg(arm.angles)) #dxl_io.set_goal_position({found_ids[0]: 0,found_ids[2]: 0,found_ids[1]: 0}) for i in range(100): dxl_io.set_moving_speed({found_ids[0]:150}) dxl_io.set_goal_position({found_ids[0]: arm_deg[0]}) dxl_io.set_moving_speed({found_ids[2]:150}) dxl_io.set_goal_position({found_ids[2]: arm_deg[1]}) dxl_io.set_moving_speed({found_ids[1]:150})
print('Using the first on the list', port) dxl_io = pypot.dynamixel.DxlIO('/dev/ttyUSB0') print('Connected!') servos_ids = dxl_io.scan([1, 2, 3, 4, 5, 6]) print('Found ids:', servos_ids) arm = tinyik.Actuator([[0., -0.055, -0.11], 'y', [0., -0.04, 0.], 'x', [0., -0.123, 0.], 'x', [0., -0.103, 0.]]) while True: t1 = time.time() img = camera_thread.read() img_1 = camera_thread_1.read() img_height, img_width, _ = img.shape coords = gesture_detection.get_coord_from_detection(img, "handphone") coords_1 = gesture_detection.get_coord_from_detection(img_1, "webcam") if coords: lefthand_0 = False lefthand_1 = False lefthand_2 = False righthand_0 = False righthand_1 = False righthand_2 = False for coord in coords: x_hand, y_hand, w_hand, h_hand, _, cat_hand = coord if x_hand >= (img_width / 2): if cat_hand == 0: