block_size = 0
big_index = 0
middle_index = 0
small_index = 0
# 입력 위치 기준 (1번 위치
INPUT_X0 = 548.90
INPUT_Y0 = 149.21
INPUT_Z = 58.40
INPUT_RX, INPUT_RY, INPUT_RZ = 171.58, 179.35, 172.35
# 출력 위치 기준 (7번 위치)
OUTPUT_X0 = 546.00
OUTPUT_Y0 = -150.44
OUTPUT_Z = 170.00
OUTPUT_RX, OUTPUT_RY, OUTPUT_RZ = 90.84, 178.98, 91.88
GAP_X = 50.0
GAP_Y = 50.0
def get_input_pos(num):
col = (num - 1) % 3
row = (num - 1) // 3
x = INPUT_X0 + col * GAP_X
y = INPUT_Y0 - row * GAP_Y
return posx(x, y, INPUT_Z, INPUT_RX, INPUT_RY, INPUT_RZ)
def get_output_pos(num):
col = (num - 1) % 3
row = (num - 1) // 3
x = OUTPUT_X0 + col * GAP_X
y = OUTPUT_Y0 + row * GAP_Y
return posx(x, y, OUTPUT_Z, OUTPUT_RX, OUTPUT_RY, OUTPUT_RZ)
sorted_big_pos = [get_output_pos(i) for i in [1, 4, 7]]
sorted_middle_pos = [get_output_pos(i) for i in [2, 5, 8]]
sorted_small_pos = [get_output_pos(i) for i in [3, 6, 9]]
def gr():
set_digital_output(1, ON)
wait(0.2)
set_digital_output(2, OFF)
wait(0.5)
def dp():
set_digital_output(1, OFF)
wait(0.2)
set_digital_output(2, ON)
wait(0.5)
def move_target():
global block_size, big_index, middle_index, small_index
target_pos = None
# 0 == big, 1 == middle, 2 == small
if block_size == 0 and big_index < 3:
target_pos = sorted_big_pos[big_index]
big_index += 1
elif block_size == 1 and middle_index < 3:
target_pos = sorted_middle_pos[middle_index]
middle_index += 1
elif block_size == 2 and small_index < 3:
target_pos = sorted_small_pos[small_index]
small_index += 1
if target_pos is not None:
# 목표 위치로 이동
movel(target_pos, radius=0.0, ref=0, mod=DR_MV_MOD_ABS,
ra=DR_MV_RA_DUPLICATE, app_type=DR_MV_APP_NONE)
# 홈 위치로 아래로 내리기
movel(posx(0.00, 0.00, -110.00, 0.00, 0.00, 0.00),
radius=0.0, ref=0, mod=DR_MV_MOD_REL,
ra=DR_MV_RA_DUPLICATE, app_type=DR_MV_APP_NONE)
# 블록 놓기
dp()
# 위로 이동
movel(posx(0.00, 0.00, 110.00, 0.00, 0.00, 0.00),
radius=0.0, ref=0, mod=DR_MV_MOD_REL,
ra=DR_MV_RA_DUPLICATE, app_type=DR_MV_APP_NONE)
def check_block():
global block_size
for num in range(0, 3):
gr()
# 블록을 잡았을 경우
if get_digital_inputs(1, 2) == 3:
block_size = num
movel(posx(0.00, 0.00, 100.00, 0.00, 0.00, 0.00),
radius=0.00, ref=0, mod=DR_MV_MOD_REL,
ra=DR_MV_RA_DUPLICATE, app_type=DR_MV_APP_NONE)
move_target()
break
# 블록을 잡지 못했을 경우
else:
dp()
wait(0.40)
movel(posx(0.00, 0.00, -10.00, 0.00, 0.00, 0.00),
radius=0.00, ref=0, mod=DR_MV_MOD_REL,
ra=DR_MV_RA_DUPLICATE, app_type=DR_MV_APP_NONE)
set_singular_handling(DR_AVOID)
set_velj(60.0)
set_accj(100.0)
set_velx(250.0, 80.625, DR_OFF)
set_accx(1000.0, 322.5)
for num in range(1, 10):
movel(get_input_pos(num), radius=0.00, ref=0, mod=DR_MV_MOD_ABS,
ra=DR_MV_RA_DUPLICATE, app_type=DR_MV_APP_NONE)
check_block()
Move JX