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:
