def turret_no_detected():
img_1 = camera_thread_1.read()
img_2 = camera_thread_2.read()
coord_1 = detection_mod.get_coord_from_detection_small(img_1)
coord_2 = detection_mod.get_coord_from_detection_small(img_2)
draw_detection(img_1, coord_1, 'cam_1')
draw_detection(img_2, coord_2, 'cam_2')
if coord_1 == [] and coord_2 == []:
robot_prop.mode = 1
return 'none', None
elif coord_1 != []:
target_coord_1 = util.get_nearest_target(coord_1, 0, 0)
return 'front', target_coord_1
else:
target_coord_2 = util.get_nearest_target(coord_2, 0, 0)
return 'back', target_coord_2
#dxl_io.set_goal_position({found_ids[0]: 0,found_ids[2]: 0,found_ids[1]: 0})
dxl_io.set_moving_speed(dict(zip((3,), itertools.repeat(50))))
dxl_io.set_goal_position(dict(zip((3,), itertools.repeat(0))))
dxl_io.set_moving_speed(dict(zip((5,), itertools.repeat(50))))
dxl_io.set_goal_position(dict(zip((5,), itertools.repeat(-45))))
dxl_io.set_moving_speed(dict(zip((4,), itertools.repeat(50))))
dxl_io.set_goal_position(dict(zip((4,), itertools.repeat(-40))))
time.sleep(1)
arm_counter =0
while True:
t1=time.time()
img = camera_thread_1.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
print (x_hand-320,y_hand-240,z_hand)
if cat_hand==1:
x_real=z_hand*(x_hand-320)/375
y_real=z_hand*(y_hand-240)/375
#print("a",x_real,y_real,z_hand,w_hand,h_hand)
arm.ee = [x_real, y_real,z_hand]
arm_deg=np.round(np.rad2deg(arm.angles))
arm_deg[1]=np.maximum(arm_deg[1],-90)
#arm_deg[2]=np.maximum(arm_deg[2],150)
port = ports[0]
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