"""팔레타이징 로봇 제어 API (책 재단기 라인). [시스템 개요] 재단기에서 나온 책이 낱권씩 컨베이어를 타고 '스태커(하부 적층 장치)'로 들어와 스택으로 쌓이고, 픽업량만큼 모이면 JAKA 로봇이 포크형 그리퍼로 스택을 통째로 떠서 팔레트 위에 층층이 적재한다. 이 파일은 Flask Blueprint 로서 웹 UI(webview) 요청을 받아 아래 전부를 수행한다. - 팔레트 종류별 적재 메인 루프 (wooden_wheel / iron_wheel / wooden_Flat / one_line_*) - 로봇 상태 폴링 + 경광등/컨베이어 인터록 (get_status_thread) - 스태커 실린더 시퀀스 제어 (stacker_thread, Modbus RTU) [로봇 제어박스 I/O 맵 - IO_CABINET] DO0 : 그리퍼 누름판(수직) 실린더 1=하강(파지) / 0=상승(해제) (포크가 책 밑을 받친 상태에서 누름판이 위에서 내려와 눌러 고정) DO1 : 그리퍼 수평 실린더(적재 푸셔) 1=전진 / 0=후퇴 DO2 : 경광등 주황 (마지막 층 근접 예고) DO3 : 경광등 초록 (정상 운전) DO4 : 경광등 빨강 (정지/완료/비상) DO5 : 동력장치(컨베이어) 운전 1=운전 / 0=정지 DI0 : 스태커 책 인입 반사판 센서 (책이 들어오면 가려짐 → 리프트 사이클 트리거) DI1 : 스태커 스택 준비 반사판 센서 (선반 위 스택이 픽업량만큼 쌓이면 가려짐 → 로봇 픽업) DI2 : 스태커 리프터(수직 실린더) '위' 센서 DI3 : 스태커 리프터(수직 실린더) '아래' 센서 (완전 하강 상태여야 로봇 포크 진입 가능 + 컨베이어 재가동) ※ DI 번호는 get_robot_status() 반환값의 din 배열 인덱스 기준. [Modbus RTU (COM3, slave 1) - 스태커 실린더] coil 0 : 스태커 리프터(수직 실린더) 1=상승(책 밀어올림) / 0=하강 coil 1 : 스태커 선반(수평 실린더) 1=닫힘(스택 받침) / 0=열림(책 통과) [스태커 동작 원리 - 하부 적층(bottom-stacking)] 1) 컨베이어로 들어온 책이 인입 센서(DI0)를 가림 → 리프터가 책을 위로 밀어올림. 리프터가 하강 위치를 벗어나 있는 동안(DI3=0) 컨베이어는 일시정지된다. 2) 리프트 중 선반(coil 1)이 잠시 열려, 책(과 그 위의 기존 스택)이 선반 높이를 통과 3) 선반이 다시 닫히고 리프터가 내려오면 새 책이 스택 맨 아래에 끼워진 채 스택 전체가 선반 위에 걸쳐진다 4) 스택이 픽업량(DI1)만큼 쌓이면 로봇이 포크로 스택 밑을 받치고 누름판(DO0)으로 잡아 통째로 팔레트에 적재한다 [스레드 구조] - Flask 요청 스레드 : /robot_start, /one_line_run 요청 안에서 적재 메인 루프가 그대로 돌기 때문에 해당 HTTP 요청은 작업 종료까지 블록된다. - get_status_thread : 0.1s 주기로 로봇 상태/센서 폴링, 경광등·컨베이어 제어, 비상정지(EMO) 해제 후 홈 복귀 처리. - stacker_thread : 스태커 리프터/선반 실린더로 책을 하부 적층하는 반복 시퀀스. [진행 상태 저장] - static/json/cache.json : 일반 적재 진행 상태 (몇 층/몇 권/현재 적재 높이) - static/json/oneLine.json : 한줄쌓기 그리드별 진행 상태 → 프로그램 재시작 시 이어서 적재할 수 있도록 매 권 적재 후 저장한다. [운영 로그] - C:/palletizing_log/log_YYYYMMDD.txt (30일 경과분 자동 삭제) """ from flask import Blueprint, request, jsonify, flash import time import json import os import sys import threading from math import radians import minimalmodbus import serial import re from datetime import datetime, timedelta import socket log_folder_path = r"C:\palletizing_log" # 운영 로그 저장 폴더 # JAKA SDK(jkrc.pyd)는 프로젝트 내 SDK 폴더에서 로드한다 current_dir = os.path.dirname(os.path.abspath(__file__)) sdk_path = os.path.join(current_dir, 'SDK') sys.path.insert(0, sdk_path) import jkrc robot_ip_address = '10.5.5.100' # 현장 로봇 컨트롤러 IP # robot_ip_address = '192.168.56.102' # (테스트용 가상 컨트롤러 IP) robot = jkrc.RC(robot_ip_address) IO_CABINET = 0 # jkrc IO 타입: 제어박스(캐비닛) IO PI = 3.1415926 api_routes = Blueprint('api_routes', __name__) # ── 진행 상태 저장 파일 경로 ────────────────────────────────────────────── current_dir = os.path.dirname(os.path.abspath(__file__)) cache_json_file_path = os.path.join(current_dir, 'static/json/cache.json') # 일반 적재 진행 상태 oneLine_json_file_path = os.path.join(current_dir, 'static/json/oneLine.json') # 한줄쌓기 진행 상태 # ── 팔레트 치수/판별 기준 (mm) ──────────────────────────────────────────── wooden_wheel_pallet_height = 653 # 나무 바퀴 팔레트 높이. UI 선택값과 비교해 종류 판별에도 사용 iron_wheel_pallet_height = 633 # 철제 바퀴 팔레트 높이 pallet_width = 0 # /update_pallet 에서 UI 입력값으로 갱신됨 pallet_length = 0 pallet_height = 0 book_center_positions = [] # 그리드별 책 중심 좌표 목록 (UI에서 전달) grid_id = 0 # 한줄쌓기에서 선택된 그리드 ("grid1"~"grid4") # 최대 적재 높이 판정 기준 (책 1층 = 130mm 두께로 계산) wheel_max_current_layer_height = 720 # 바퀴 팔레트: 이 높이 초과 적재 불가 flat_max_current_layer_height = 1080 # 납작 팔레트 oneline_wheel_max_current_layer_height = 720 # 한줄쌓기 # ── 동작 상태 플래그 (스레드 간 공유 전역) ──────────────────────────────── robot_start_clicked = False # UI '시작' 눌림 여부 (경광등 초록 조건) terminate_robot_button_clicked = False # 작업 종료(완료/정지) 상태 stacker_thread_running = False # 스태커 스레드 동작 여부 stacker_thread_operator = None # 스태커 스레드 핸들 (종료 시 join 용) action = None # UI '로봇 동작 종료' 명령 ('robot_finish_action') immediately_take_book = None # UI '즉시 픽업' 명령 (책 준비 센서 무시하고 픽업) is_one_line_stop_clicked = None # UI '한줄쌓기 멈춤' 명령 btn_id = None # 'normalButton'(일반 적재) / 'oneLineButton'(한줄쌓기) flag = 0 # 비상정지 발생 이력 (1이면 해제 시 홈 복귀 필요) is_emo_disabled = True # True = 비상정지 아님(정상). EMO 복귀 완료 시 True 복원 # ── 모션 파라미터 ──────────────────────────────────────────────────────── # ※ SDK 단위: 직선 이동 speed=mm/s, acc=mm/s² / 관절 이동 speed=rad/s, acc=rad/s² # 아래 값은 컨트롤러 상한을 넘으므로 사실상 "허용 최대 속도"로 동작한다. speed = 7000 # 직선 이동 속도 (책 리프트/적재 하강 등) acc = 2300 # 직선 이동 가속도 # 그리드 진입(책 실은 채 적재 지점 상공으로 가는) 관절 이동 파라미터. # acc=1(rad/s²)로 가감속을 매우 완만하게 하여 책 미끄러짐을 방지한다. # ★ 택타임 튜닝 포인트: 책이 안 미끄러지는 범위에서 grid_acc를 올리면 사이클 단축 가능. grid_speed = 10 grid_acc = 1 # ═════════════════ 대기시간 설정 (택타임 튜닝 포인트) ═════════════════════ # 값을 줄이면 사이클이 빨라지지만, 실린더 스트로크가 끝나기 전에 다음 동작이 # 시작되면 책 낙하/밀림이 생길 수 있다. 문제 발생 시 주석의 [기존값]으로 복원할 것. GRIPPER_RETRACT_WAIT = 0.5 # [기존 2.0] 픽업 진입 전 그리퍼 실린더 후퇴 확보 대기. # 직전 사이클부터 이미 OFF 상태로 수 초간 이동해 왔으므로 # 재확인 성격. 문제 발생 시 1.0 → 2.0 순으로 복원. GRIP_CLAMP_WAIT_ODD = 0.3 # [기존 0.3] 홀수층 파지: 수직 그리퍼 ON 후 파지 완료 대기. GRIP_CLAMP_WAIT_EVEN = 0.1 # [기존 0.1] 짝수층 파지: 〃. 짝수층은 0.1로 운영 검증되어 # 있으므로 홀수층(0.3)도 단계적으로 줄여볼 여지 있음. BOOK_SETTLE_WAIT = 0.3 # [기존 0.3] 책 들어올린 직후 흔들림 안정화 대기. RELEASE_WAIT = 0.2 # [기존 0.3/0.2] 적재 후 수직 그리퍼 OFF→후퇴 대기 (0.2로 통일). PUSH_HOLD_WAIT = 0.6 # [기존 1.0] 수평 푸셔 ON 후 스트로크 완료 대기. FORK_EXIT_WAIT = 0.3 # [기존 0.5] 포크 인출(-180mm) 후 푸셔 해제 전 대기. IN_POS_GUARD_TIME = 0.05 # [기존 0.1] 모션 명령 직후 in-pos 오판(이전 위치 기준 1 반환) # 방지 가드. 책 1권당 8~10회 호출되므로 누적 영향이 큼. IN_POS_POLL_INTERVAL = 0.01 # [신규] is_in_pos() 폴링 주기 (기존: 간격 없는 busy-wait) IDLE_LOOP_INTERVAL = 0.02 # [신규] 메인 루프 유휴 폴링 주기 (기존: 간격 없는 busy-wait) JKS_POLL_INTERVAL = 0.05 # [신규] 높이탐색 JKS 프로그램 종료 폴링 주기 # ── 스태커 실린더 시퀀스 대기 (물리 스트로크 시간, 기존값 유지) ── STACKER_BACKOFF = 0.5 # [기존 2.0] 스택 픽업 대기(DI1 가려짐) 상태에서 재확인 주기 STACKER_LIFT_START_WAIT = 0.5 # 책 인입 감지 후 리프트 시작 전 대기 (책이 완전히 들어올 시간) STACKER_SHELF_OPEN_DELAY = 0.05 # 리프트 시작 후 선반 열기까지 지연 STACKER_SHELF_OPEN_TIME = 1.2 # 선반 열림 유지 시간 (리프터가 스택을 선반 위로 올리는 동안) STACKER_SHELF_CLOSE_SETTLE = 0.2 # 선반 닫힘 후 리프터 하강 전 대기 (스택 안착) # ═══════════════════════════════════════════════════════════════════════ # ── 스태커 센서 상태 (get_status_thread 가 0.1s 주기로 갱신) ────────────── stacker_vertical_cyl_sensor = 0 # DI2: 리프터(수직 실린더) '위' 센서 stacker_vertical_cyl_down_sensor = 0 # DI3: 리프터(수직 실린더) '아래' 센서 stacker_book_ready_sensor = 0 # DI1: 스택 준비(픽업량 도달) 센서 # ── 팔레트별 최대 하강 z (사용자좌표계 기준 거리) ───────────────────────── wooden_wheel_pallet_max_z_pos = 864 iron_wheel_pallet_max_z_pos = 884 wooden_Flat_pallet_max_z_pos = 1192 # 기준 팔레트 높이 227mm 일 때 값. /update_pallet 에서 보정됨 # ── Modbus RTU (스태커 실린더 제어) 설정 ───────────────────────────────── port = 'COM3' baudrate = 115200 timeout = 2 slave_id = 1 # Modbus 슬레이브 ID start_address = 0x0000 # 코일 시작 주소 num_coils = 8 # 코일 개수 instrument = minimalmodbus.Instrument(port, slave_id) instrument.serial.baudrate = baudrate instrument.serial.timeout = timeout instrument.mode = minimalmodbus.MODE_RTU instrument.close_port_after_each_call = True # 매 호출 후 COM 포트 자동 반환 (타 프로그램과 공유 가능) def Write_All_Do(coils_values): """스태커 Modbus 코일 8개를 한 번에 쓴다 (Function Code 15). coils_values: 길이 8 리스트. [0]=수직 실린더, [1]=수평 실린더, 나머지 예비. """ try: instrument.write_bits(start_address, coils_values) print(f"Successfully wrote {num_coils} coils to slave {slave_id}") except IOError as e: print(f"Error writing coils: {e}") Write_All_Do([0, 0, 0, 0, 0, 0, 0, 0]) # 앱 시작 시 스태커 실린더 전체 OFF (안전 초기화) def write_log(message): """운영 로그 파일(C:/palletizing_log/log_YYYYMMDD.txt)에 한 줄 기록한다.""" try: # 폴더 없으면 생성 if not os.path.exists(log_folder_path): os.makedirs(log_folder_path) #현재 날짜로 파일 이름 설정 current = datetime.now().strftime("%Y%m%d") filename = f"log_{current}.txt" log_file_path = os.path.join(log_folder_path, filename) #현재 시간 current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S") #적을 내용 msg = f"[{current_time}] {message}\n" #로그 파일에 메시지 기록 with open(log_file_path, "a") as log_file: log_file.write(msg) #30일전 로그 삭제 delete_old_logs(30) return True except Exception as e: #에러 print(f"Error log: {e}") return False def delete_old_logs(days): """days 일보다 오래된 로그 파일을 삭제한다.""" try: #현재 날짜 - days 계산 deletion_date = datetime.now() - timedelta(days=days) #폴더 내의 모든 파일 검사 for filename in os.listdir(log_folder_path): #파일 이름 날짜 추출 if filename.startswith("log_") and filename.endswith(".txt"): file_date_str = filename[4:12] # "log_YYYYMMDD.txt"에서 YYYYMMDD 추출 file_date = datetime.strptime(file_date_str, "%Y%m%d") #기준일보다 오래된 경우 파일 삭제 if file_date < deletion_date: file_path = os.path.join(log_folder_path, filename) os.remove(file_path) print(f"Deleted log: {file_path}") except Exception as e: #에러 print(f"Error deleting logs: {e}") # ═════════════════════ HTTP 라우트 (웹 UI ↔ 백엔드) ═════════════════════ @api_routes.route('/writelog', methods=["POST"]) def writelog(): """프론트엔드가 보낸 메시지를 운영 로그 파일에 기록한다.""" try: #적을 내용 message = request.get_data(as_text=True) #메시지가 없을 때 if not message: return "Not Message" # 로그 기록 if write_log(message): return "Success" # 실패 else: return "Failed" #실패 except Exception as e: return "Failed to " + str(e) @api_routes.route('/update_pallet', methods=['GET', 'POST']) def update_pallet(): """UI에서 입력한 팔레트 규격(폭/길이/높이)을 반영한다. - 납작 팔레트 최대 하강값(wooden_Flat_pallet_max_z_pos)을 높이에 맞게 보정 - 팔레트 원점 기준 사용자좌표계(6번)를 재설정 """ global pallet_width, pallet_length, pallet_height, wooden_Flat_pallet_max_z_pos data = request.json pallet_width = data.get('palletWidth') pallet_length = data.get('palletLength') pallet_height = data.get('palletHeight') wooden_Flat_pallet_max_z_pos = 1192 # 227 wooden_Flat_pallet_max_z_pos = wooden_Flat_pallet_max_z_pos - (pallet_height - 227) print(wooden_Flat_pallet_max_z_pos) print(pallet_height) l = pallet_width / 2 x_cord = pallet_length err_sum = 188 + 18 # 188 : 책 놓았을때 오차 / 18 : 팔레트 width 오차 new_coord_data = [-l - err_sum, 881.7 + x_cord, -764 + pallet_height, 0, -PI, -PI] change_user_frame_coordiante(new_coord_data) write_log(f"BE: update_pallet function successfully completed") return 'Success' global selectedPalletId # 선택된 팔레트 종류 문자열 (robot_start 에서 설정) @api_routes.route('/robot_start', methods=['POST']) def robot_start(): """시작 버튼 처리: 팔레트 종류를 판별해 해당 적재 함수를 실행한다. - normalButton : 적재 메인 루프가 이 요청 안에서 그대로 돌기 때문에 작업이 끝날 때까지 HTTP 응답이 반환되지 않는다. - oneLineButton: 책 좌표만 저장하고, 실제 실행은 /one_line_run 에서 한다. """ global robot_start_clicked global book_center_positions global terminate_robot_button_clicked global action global btn_id global selectedPalletId data = request.json robot_start_clicked = True terminate_robot_button_clicked = False action = None pallet_height = data['selectedPallet']['palletHeight'] btn_id = data['btnId'] selectedPalletId = data['selectedPalletId'] print("selectedPalletId : ", selectedPalletId) print(f"pallet_height: {pallet_height}, btn_id: {btn_id}") write_log(f"BE: robot_start function start - pallet_height: {pallet_height}, btn_id: {btn_id}") if data: if btn_id == 'normalButton': stacker_thread_start_function() if pallet_height == wooden_wheel_pallet_height: book_center_positions = data['selectedPallet']['bookCenterPosition_Wheel'] wooden_wheel_pallet() elif pallet_height == iron_wheel_pallet_height: book_center_positions = data['selectedPallet']['bookCenterPosition_Wheel'] iron_wheel_pallet() else: book_center_positions = data['selectedPallet']['bookCenterPosition_Flat'] print("납작 함수 호출!!!!!!!!!!!!!!!!!!!") wooden_Flat_pallet() write_log("BE: robot_start function successfully completed") return 'Success' elif btn_id == 'oneLineButton': if pallet_height == wooden_wheel_pallet_height: book_center_positions = data['selectedPallet']['bookCenterPosition_Wheel'] elif pallet_height == iron_wheel_pallet_height: book_center_positions = data['selectedPallet']['bookCenterPosition_Wheel'] write_log("BE: robot_start function successfully completed") return 'Success' else: return 'Error: No data' @api_routes.route('/robot_finish_action', methods=['POST']) def robot_finish_action(): """UI '로봇 동작 종료' 버튼: 메인 루프에 종료를 알리고 한줄쌓기 상태를 초기화한다.""" global action req_data = request.json action = req_data.get('action') if action == 'robot_finish_action': oneLine_data = { "grid1": {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid2": {"currentDataIndex": 1, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid3": {"currentDataIndex": 2, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid4": {"currentDataIndex": 3, "currentLayerIndex": 0, "currentLayerHeight": 0} } with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot_finish_action function successfully completed") return 'Robot finish action successful' else: return 'Invalid action', 400 @api_routes.route('/robot_immediately_take_book', methods=['POST']) def robot_immediately_take_book(): """UI '즉시 픽업' 버튼: 책 준비 센서를 기다리지 않고 다음 사이클에 바로 픽업시킨다.""" global immediately_take_book req_data = request.json immediately_take_book = req_data.get('immediately_take_book') if immediately_take_book == 'robot_immediately_take_book': write_log("BE: robot_immediately_take_book function successfully completed") return 'robot_immediately_take_book successful' else: return 'Invalid action', 400 @api_routes.route('/reset_data', methods=['POST']) def reset_data(): """UI '리셋' 버튼: 일반/한줄쌓기 적재 진행 상태(json)를 모두 초기화한다.""" data = request.json if data.get('message') == 'Reset button clicked': cache_data = { "currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0 } with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) oneLine_data = { "grid1": {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid2": {"currentDataIndex": 1, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid3": {"currentDataIndex": 2, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid4": {"currentDataIndex": 3, "currentLayerIndex": 0, "currentLayerHeight": 0} } with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) write_log("BE: reset_data function successfully completed") return 'Success' else: return 'Error: Invalid message' @api_routes.route('/one_line_run', methods=['POST']) def one_line_run(): """한줄쌓기 시작: 선택 그리드(grid_id)에 대해 팔레트 종류별 한줄쌓기 루프 실행. 사전에 /update_pallet, /robot_start(oneLineButton)로 규격·좌표가 설정되어 있어야 한다. normalButton 과 마찬가지로 작업 종료까지 HTTP 응답이 블록된다. """ global grid_id global terminate_robot_button_clicked global action global robot_start_clicked global is_one_line_stop_clicked robot_start_clicked = True terminate_robot_button_clicked = False action = None is_one_line_stop_clicked = None data = request.json grid_id = data.get('gridId') write_log(f"BE: one_line_run function start - pallet_height: {pallet_height}, grid_id: {grid_id}") stacker_thread_start_function() if data.get('message') == 'one_line_run': if pallet_height == wooden_wheel_pallet_height: one_line_wooden_wheel_pallet() elif pallet_height == iron_wheel_pallet_height: one_line_iron_wheel_pallet() write_log("BE: one_line_run function successfully completed") return 'Success' else: return 'Error: Invalid message' @api_routes.route('/one_line_stop', methods=['POST']) def one_line_stop(): """UI '한줄쌓기 멈춤' 버튼: 현재 사이클 완료 후 루프를 종료시킨다.""" global is_one_line_stop_clicked data = request.json is_one_line_stop_clicked = data.get('message') if data.get('message') == 'one_line_stop': write_log("BE: one_line_stop function successfully completed") return 'Success' else: return 'Error: Invalid message' @api_routes.route('/stacker_start', methods=['POST']) def stacker_start(): """스태커 수동 시작 (이미 동작 중이면 무시).""" data = request.json if data.get('action') == 'stacker_start': if stacker_thread_running: print("이미 스태커가 실행 중입니다.") write_log("BE: stacker_start function successfully completed 1") return 'Success' else: # robot.set_digital_output(IO_CABINET, 2, 0) instrument.write_bit(0, False) stacker_thread_start_function() print("스태커 실행") write_log("BE: stacker_start function successfully completed 2") return 'Success' else: return jsonify({'error': 'Invalid message'}), 400 @api_routes.route('/stacker_stop', methods=['POST']) def stacker_stop(): """스태커 수동 정지.""" global stacker_thread_running data = request.json if data.get('action') == 'stacker_stop': if stacker_thread_running: stacker_thread_running = False print("스태커 멈춤") write_log("BE: stacker_stop function successfully completed 1") return 'Success' else: print("이미 스태커가 멈춰있습니다.") write_log("BE: stacker_stop function successfully completed 2") return 'Success' else: return jsonify({'error': 'Invalid message'}), 400 @api_routes.route('/updown_on', methods=['POST']) def updown_on(): """스태커 수직 실린더 수동 ON (스태커 자동 동작 중에는 거부).""" global stacker_thread_running data = request.json print(data) if data.get('action') == 'updown_on' and stacker_thread_running == False: # robot.set_digital_output(IO_CABINET, 2, 1) instrument.write_bit(0, True) print("수직 실린더 on") write_log("BE: updown_on function successfully completed") return 'Success' else: return jsonify({'error': 'Invalid message'}), 400 @api_routes.route('/updown_off', methods=['POST']) def updown_off(): """스태커 수직 실린더 수동 OFF (스태커 자동 동작 중에는 거부).""" global stacker_thread_running data = request.json print(data) if data.get('action') == 'updown_off' and stacker_thread_running == False: # robot.set_digital_output(IO_CABINET, 2, 0) instrument.write_bit(0, False) print("수직 실린더 off") write_log("BE: updown_off function successfully completed") return 'Success' else: return jsonify({'error': 'Invalid message'}), 400 @api_routes.route('/leftright_on', methods=['POST']) def leftright_on(): """스태커 수평 실린더 수동 ON (스태커 자동 동작 중에는 거부).""" global stacker_thread_running data = request.json print(data) if data.get('action') == 'leftright_on' and stacker_thread_running == False: # robot.set_digital_output(IO_CABINET, 3, 1) instrument.write_bit(1, True) print("수평 실린더 on") write_log("BE: leftright_on function successfully completed") return 'Success' else: return jsonify({'error': 'Invalid message'}), 400 @api_routes.route('/leftright_off', methods=['POST']) def leftright_off(): """스태커 수평 실린더 수동 OFF (스태커 자동 동작 중에는 거부).""" global stacker_thread_running data = request.json if data.get('action') == 'leftright_off' and stacker_thread_running == False: # robot.set_digital_output(IO_CABINET, 3, 0) instrument.write_bit(1, False) print("수평 실린더 off") write_log("BE: leftright_off function successfully completed") return 'Success' else: return jsonify({'error': 'Invalid message'}), 400 def change_user_frame_coordiante(new_coord_data): """사용자좌표계 6번('ScreenFlask')을 새 팔레트 기준 원점으로 설정한다. 모든 적재 모션(linear_move 등)이 이 좌표계 기준으로 동작하므로, 팔레트 규격이 바뀌면 /update_pallet 를 통해 반드시 갱신되어야 한다. """ robot.login() robot.power_on() robot.enable_robot() robot.set_user_frame_data(6, new_coord_data, "ScreenFlask") write_log("BE: change_user_frame_coordiante function successfully completed") return 'Success' def wooden_wheel_pallet(): """[일반 적재] 나무 바퀴 팔레트: 층당 4권을 ㅁ자(井형) 배치로 적재. 책 1권 = 1사이클 처리 흐름: 대기 자세 → 스태커 '책 준비' 센서 대기 → 픽업(진입-파지-리프트) → 팔레트 진입 자세 → 그리드 상공 이동 → 하강 + 높이 탐색 → 릴리즈 → 홈 복귀 → 진행 상태(cache.json) 저장 - 홀수층/짝수층은 그리퍼 방향을 반대로 하여 맞물리게 쌓는다. - 최대 적재 높이 도달 또는 UI '동작 종료' 시 루프를 빠져나간다. - 비상정지 시 EMO()가 True를 반환하며 사이클을 중단하고, 해제되면 get_status_thread 가 홈 복귀시킨 뒤 대기 자세부터 다시 진행한다. """ write_log("BE: wooden_wheel_pallet function started") global action, immediately_take_book global emergency_stop, protected_stop_stat global power_on_stat, enabled_stat global robot_start_clicked, terminate_robot_button_clicked global pallet_width, pallet_length, pallet_height, book_center_positions global stacker_thread_running, stacker_thread_operator global is_emo_disabled, stacker_vertical_cyl_sensor, stacker_book_ready_sensor global z_pos global wooden_wheel_pallet_max_z_pos, stacker_vertical_cyl_down_sensor is_move_pose = True robot.linear_move_extend([0, 0, -350, 0, 0, 0], 1, True, 7000, 2000, 0.1) write_log("BE: linear_move_extend : [0, 0, -350, 0, 0, 0] successfully completed - wooden_wheel_pallet") home_pos = [radians(-58.423), radians(95.567), radians(66.994), radians(107.440), radians(-90), radians(31.577)] lower_pallet_enter_pose = [radians(-61.533), radians(107.484), radians(55.576), radians(106.940), radians(-90), radians(-61.533)] lower_pallet_enter_pose1 = [radians(-66.797), radians(136.143), radians(6.799), radians(127.058), radians(-90), radians(-66.797)] robot.login() write_log("BE: login successfully completed - wooden_wheel_pallet") robot.power_on() write_log("BE: power_on successfully completed - wooden_wheel_pallet") robot.enable_robot() write_log("BE: enable_robot successfully completed - wooden_wheel_pallet") robot.joint_move_extend(home_pos, 0, True, 10, 1, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_wheel_pallet") if os.path.exists(cache_json_file_path) and os.path.getsize(cache_json_file_path) > 0: with open(cache_json_file_path, 'r', encoding='utf-8') as file: cache_data = json.load(file) current_data_index_cache = cache_data.get('currentDataIndex') current_layer_index_cache = cache_data.get('currentLayerIndex') current_layer_height_cache = cache_data.get('currentLayerHeight') current_data_index = current_data_index_cache current_layer_index = current_layer_index_cache current_layer_height = current_layer_height_cache else: cache_data = { "currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0 } with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) current_data_index = 0 current_layer_index = 0 current_layer_height = 0 while True: # busy-wait 방지: 짧게 쉬어 상태 폴링 스레드(GIL)가 센서값을 제때 갱신하도록 함 time.sleep(IDLE_LOOP_INTERVAL) if (EMO(0, 0)): is_move_pose = True continue # UI '동작 종료' → 진행 상태 초기화 후 루프 종료 (스태커/컨베이어 정지) if action == 'robot_finish_action': action = None current_data_index = 0 current_layer_index = 0 current_layer_height = 0 terminate_robot_button_clicked = True cache_data['currentLayerIndex'] = current_layer_index cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 4, 0) (conveyor belt stopped) successfully completed - wooden_wheel_pallet") write_log("BE: action == 'robot_finish_action' successfully completed - wooden_wheel_pallet") break # 다음 층(130mm)을 쌓으면 최대 높이 초과 → 팔레트 완료 처리 후 종료 if current_layer_height + 130 > wheel_max_current_layer_height and current_data_index == 4: action = None current_data_index = 0 current_layer_index = 0 current_layer_height = 0 terminate_robot_button_clicked = True cache_data['currentLayerIndex'] = current_layer_index cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt stopped) successfully completed - wooden_wheel_pallet") write_log("BE: current_layer_height > wheel_max_current_layer_height successfully completed - wooden_wheel_pallet") break # 4권 다 놓으면 레이어 index + 1 if(current_data_index == 4): current_layer_index += 1 current_data_index = 0 cache_data['currentLayerIndex'] = current_layer_index cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) # ── 픽업 대기 자세 잡기 (사이클 완료/비상 복구 후 1회씩 수행) ── # 홈 → 픽업 준비 자세 → 픽업 대기 높이 하강 순서로 이동해 두고 책을 기다린다. if is_move_pose == True and is_emo_disabled == True: is_move_pose = False if current_layer_index % 2 == 0: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_wheel_pallet") is_in_pose() pickup_pose = [radians(-47.102), radians(84.387), radians(78.169), radians(107.444), radians(-90), radians(131.510)] robot.joint_move_extend(pickup_pose, 0, False, 8000, 3000, 0.1) # 책 뜨기 공중 준비 자세 write_log("BE: joint_move_extend : pickup_pose successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 666.816, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(스태커 옆 공중 대기) write_log("BE: linear_move_extend : [0,0,666.816,0,0,0] (ready pose) successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue else: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_wheel_pallet") is_in_pose() pickup_pose_opposite = [radians(-24.283), radians(120.475), radians(32.213), radians(117.312), radians(-90), radians(-25.671)] robot.joint_move_extend(pickup_pose_opposite, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : pickup_pose_opposite successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 681.817, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(짝수층 대기 위치) write_log("BE: linear_move_extend : [0,0,681.817,0,0,0] (ready pose) successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue # ── 픽업 시작 조건 ── # (스택 픽업량 도달(DI1) AND 리프터 완전 하강(DI3) AND 로봇 정상) # 또는 (UI '즉시 픽업' AND 리프터 상승 상태 아님 AND 로봇 정상) if (((stacker_book_ready_sensor == 1) and (stacker_vertical_cyl_sensor == 0) and (stacker_vertical_cyl_down_sensor == 1)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)) or (((immediately_take_book == 'robot_immediately_take_book') and (stacker_vertical_cyl_sensor == 0)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)): immediately_take_book = None if current_layer_index % 2 == 0: # 홀수 번째 층(1,3,5...): 기본 방향으로 픽업 write_log("BE: load book on pallet's if condition started - wooden_wheel_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - wooden_wheel_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - wooden_wheel_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([-298.306, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (포크가 책 밑으로 들어감) write_log("BE: linear_move_extend : [-298.306, 0, 0,0,0,0] (move toward stacker to grip book) successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - wooden_wheel_pallet") time.sleep(GRIP_CLAMP_WAIT_ODD) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -694.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0, 0, -694.816,0,0,0] (lift with book placed on gripper) successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY > pallet_length / 2: # 아래 팔레트 진입 자세 robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose for odd layer grid #2 and #4 successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 1: # grid 2 robot.joint_move_extend([radians(-83.398), radians(102.810), radians(61.767), radians(105.424), radians(-90), radians(-85.698)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_wheel_pallet(odd layer) grid 2 Ready pose/ joint_move_extend [radians(-83.398), radians(102.810), radians(61.767), radians(105.424), radians(-90), radians(-85.698)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 31, centerY + 9, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend : [{centerX + 31}, {centerY + 9}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) if current_data_index == 3: # grid 4 robot.joint_move_extend([radians(-66.699), radians(105.342), radians(58.472), radians(106.187), radians(-90), radians(-68.999)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_wheel_pallet(odd layer) grid 4 Ready pose/ joint_move_extend[[radians(-62.833), radians(107.457), radians(55.613), radians(106.930), radians(-90), radians(25.779)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 24, centerY + 9, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2: [{centerX + 24}, {centerY + 9}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) else: # 위에 팔레트 진입 자세 robot.joint_move_extend(lower_pallet_enter_pose1, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose1 for grid 1 and 3 successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # grid 1 robot.joint_move_extend([radians(-84.575), radians(122.286), radians(32.836), radians(114.878), radians(-90), radians(-86.875)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_wheel_pallet(odd layer) grid 1 Ready pose/ joint_move_extend [radians(-84.575), radians(122.286), radians(32.836), radians(114.878), radians(-90), radians(-86.875)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 31, centerY + 5, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 31}, {centerY + 5}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) if current_data_index == 2: # grid 3 robot.joint_move_extend([radians(-70.674), radians(125.675), radians(26.930), radians(117.394), radians(-90), radians(-72.974)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_wheel_pallet(odd layer) grid 3 Ready pose/ joint_move_extend [radians(-70.674), radians(125.675), radians(26.930), radians(117.394), radians(-90), radians(-72.974)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 24, centerY + 5, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3 : [{centerX + 24}, {centerY + 5}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) current_layer_height = z_pos # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - wooden_wheel_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - wooden_wheel_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -180, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [적재] 포크만 위로 -180mm 인출 write_log(f"BE: linear_move_extend : [0, 0, -180, 0, 0, 0] (move robot little bit up) successfully completed - wooden_wheel_pallet") is_in_pose() time.sleep(FORK_EXIT_WAIT) # (기존 0.5s) 책 안정화 후 푸셔 해제 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - wooden_wheel_pallet") if (EMO(1, 0)): continue robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(1, 0)): continue current_data_index += 1 cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) is_move_pose = True else: # 짝수 번째 층(2,4,6...): 그리퍼를 돌려 반대 방향 픽업(맞물림 적재로 하중 분산) write_log("BE: load book on pallet's else condition started - wooden_wheel_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - wooden_wheel_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - wooden_wheel_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([194.344, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (짝수층: 반대 방향에서 진입) write_log("BE: linear_move_extend : [194.344,0,0,0,0,0] (move toward stacker to grip book) successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - wooden_wheel_pallet") time.sleep(GRIP_CLAMP_WAIT_EVEN) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -703.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0,0,-703.816,0,0,0] (lift with book placed on gripper) successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY > pallet_length /2: robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 5, 1, 0.1) # 짝수층은 파지 자세 특성상 저속 진입(책 미끄러짐 방지). 미끄러지면 speed/acc를 더 낮출 것 write_log("BE: joint_move_extend : lower_pallet_enter_pose for even layer grid 2 and 4 successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 1: # grid 2 robot.joint_move_extend([radians(-83.398), radians(102.810), radians(61.767), radians(105.424), radians(-90), radians(-85.698)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_wheel_pallet(even layer) grid 2 Ready pose/ joint_move_extend [radians(-83.398), radians(102.810), radians(61.767), radians(105.424), radians(-90), radians(-85.698)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 31, centerY + 9, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2 : [{centerX + 31}, {centerY + 9}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) if current_data_index == 3: # grid 4 robot.joint_move_extend([radians(-66.699), radians(105.342), radians(58.472), radians(106.187), radians(-90), radians(-68.999)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_wheel_pallet(even layer) grid 4 Ready pose/ joint_move_extend [radians(-66.699), radians(105.342), radians(58.472), radians(106.187), radians(-90), radians(-68.999)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 24, centerY + 9, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 4 : [{centerX + 24}, {centerY + 9}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) else: robot.joint_move_extend(lower_pallet_enter_pose1, 0, False, 5, 1, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose1 for grid 1 and 3 successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # grid 1 robot.joint_move_extend([radians(-84.575), radians(122.286), radians(32.836), radians(114.878), radians(-90), radians(-86.875)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_wheel_pallet(even layer) grid 1 Ready pose/ joint_move_extend [radians(-84.575), radians(122.286), radians(32.836), radians(114.878), radians(-90), radians(-86.875)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 31, centerY + 5, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 31}, {centerY + 5}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) if current_data_index == 2: # grid 3 robot.joint_move_extend([radians(-70.674), radians(125.675), radians(26.930), radians(117.394), radians(-90), radians(-72.974)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_wheel_pallet(even layer) grid 3 Ready pose/ joint_move_extend [radians(-70.674), radians(125.675), radians(26.930), radians(117.394), radians(-90), radians(-72.974)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 24, centerY + 5, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3: [{centerX + 24}, {centerY + 5}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) current_layer_height = z_pos # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - wooden_wheel_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - wooden_wheel_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -180, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [적재] 포크만 위로 -180mm 인출 write_log(f"BE: linear_move_extend : [0, 0, -180, 0, 0, 0] (move robot little bit up) successfully completed - wooden_wheel_pallet") is_in_pose() time.sleep(FORK_EXIT_WAIT) # (기존 0.5s) 책 안정화 후 푸셔 해제 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - wooden_wheel_pallet") if (EMO(1, 0)): continue robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_wheel_pallet") is_in_pose() if (EMO(1, 0)): continue current_data_index += 1 cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) is_move_pose = True def iron_wheel_pallet(): """[일반 적재] 철제 바퀴 팔레트: 층당 4권 ㅁ자 배치. wooden_wheel_pallet() 과 동일한 시퀀스. 차이점: - 최대 하강 z: iron_wheel_pallet_max_z_pos(884) - 그리드 진입 자세/적재 좌표 오프셋이 철제 팔레트 실측 기준 """ write_log("BE: iron_wheel_pallet function started") global action, immediately_take_book global emergency_stop, protected_stop_stat global power_on_stat, enabled_stat global robot_start_clicked, terminate_robot_button_clicked global pallet_width, pallet_length, pallet_height, book_center_positions global stacker_thread_running, stacker_thread_operator global is_emo_disabled, stacker_vertical_cyl_sensor, stacker_book_ready_sensor, stacker_vertical_cyl_down_sensor # @@@ 새로 추가한 변수 global z_pos global iron_wheel_pallet_max_z_pos # @@@ is_move_pose = True robot.linear_move_extend([0, 0, -350, 0, 0, 0], 1, True, 7000, 2000, 0.1) write_log("BE: linear_move_extend : [0, 0, -350, 0, 0, 0] successfully completed - iron_wheel_pallet") home_pos = [radians(-58.423), radians(95.567), radians(66.994), radians(107.440), radians(-90), radians(31.577)] lower_pallet_enter_pose = [radians(-61.533), radians(107.484), radians(55.576), radians(106.940), radians(-90), radians(-61.533)] lower_pallet_enter_pose1 = [radians(-66.797), radians(136.143), radians(6.799), radians(127.058), radians(-90), radians(-66.797)] robot.login() write_log("BE: login successfully completed - iron_wheel_pallet") robot.power_on() write_log("BE: power_on successfully completed - iron_wheel_pallet") robot.enable_robot() write_log("BE: enable_robot successfully completed - iron_wheel_pallet") robot.joint_move_extend(home_pos, 0, True, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - iron_wheel_pallet") if os.path.exists(cache_json_file_path) and os.path.getsize(cache_json_file_path) > 0: with open(cache_json_file_path, 'r', encoding='utf-8') as file: cache_data = json.load(file) current_data_index_cache = cache_data.get('currentDataIndex') current_layer_index_cache = cache_data.get('currentLayerIndex') current_layer_height_cache = cache_data.get('currentLayerHeight') current_data_index = current_data_index_cache current_layer_index = current_layer_index_cache current_layer_height = current_layer_height_cache else: cache_data = { "currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0 } with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) current_data_index = 0 current_layer_index = 0 current_layer_height = 0 while True: # busy-wait 방지: 짧게 쉬어 상태 폴링 스레드(GIL)가 센서값을 제때 갱신하도록 함 time.sleep(IDLE_LOOP_INTERVAL) if (EMO(0, 0)): is_move_pose = True continue # UI '동작 종료' → 진행 상태 초기화 후 루프 종료 (스태커/컨베이어 정지) if action == 'robot_finish_action': action = None current_data_index = 0 current_layer_index = 0 current_layer_height = 0 terminate_robot_button_clicked = True cache_data['currentLayerIndex'] = current_layer_index cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - iron_wheel_pallet") write_log("BE: action == 'robot_finish_action' successfully completed - iron_wheel_pallet") break # 다음 층(130mm)을 쌓으면 최대 높이 초과 → 팔레트 완료 처리 후 종료 if current_layer_height + 130 > wheel_max_current_layer_height and current_data_index == 4: action = None current_data_index = 0 current_layer_index = 0 current_layer_height = 0 terminate_robot_button_clicked = True cache_data['currentLayerIndex'] = current_layer_index cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - iron_wheel_pallet") write_log("BE: current_layer_height > wheel_max_current_layer_height successfully completed - iron_wheel_pallet") break # 4권 다 놓으면 레이어 index + 1 if(current_data_index == 4): current_layer_index += 1 current_data_index = 0 cache_data['currentLayerIndex'] = current_layer_index cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) # ── 픽업 대기 자세 잡기 (사이클 완료/비상 복구 후 1회씩 수행) ── # 홈 → 픽업 준비 자세 → 픽업 대기 높이 하강 순서로 이동해 두고 책을 기다린다. if is_move_pose == True and is_emo_disabled == True: is_move_pose = False if current_layer_index % 2 == 0: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - iron_wheel_pallet") is_in_pose() pickup_pose = [radians(-47.102), radians(84.387), radians(78.169), radians(107.444), radians(-90), radians(131.510)] robot.joint_move_extend(pickup_pose, 0, False, 8000, 3000, 0.1) # 책 뜨기 공중 준비 자세 write_log("BE: joint_move_extend : pickup_pose successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 666.816, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(스태커 옆 공중 대기) write_log("BE: linear_move_extend : [0,0,666.816,0,0,0] (ready pose) successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue else: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - iron_wheel_pallet") is_in_pose() pickup_pose_opposite = [radians(-24.283), radians(120.475), radians(32.213), radians(117.312), radians(-90), radians(-25.671)] robot.joint_move_extend(pickup_pose_opposite, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : pickup_pose_opposite successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 681.817, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(짝수층 대기 위치) write_log("BE: linear_move_extend : [0,0,681.817,0,0,0] (ready pose) successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue # ── 픽업 시작 조건 ── # (스택 픽업량 도달(DI1) AND 리프터 완전 하강(DI3) AND 로봇 정상) # 또는 (UI '즉시 픽업' AND 리프터 상승 상태 아님 AND 로봇 정상) if (((stacker_book_ready_sensor == 1) and (stacker_vertical_cyl_sensor == 0) and (stacker_vertical_cyl_down_sensor == 1)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)) or (((immediately_take_book == 'robot_immediately_take_book') and (stacker_vertical_cyl_sensor == 0)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)): immediately_take_book = None if current_layer_index % 2 == 0: # 홀수 번째 층(1,3,5...): 기본 방향으로 픽업 write_log("BE: load book on pallet's if condition started - iron_wheel_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - iron_wheel_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - iron_wheel_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([-298.306, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (포크가 책 밑으로 들어감) write_log("BE: linear_move_extend : [-298.306, 0, 0,0,0,0] (move toward stacker to grip book) successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - iron_wheel_pallet") time.sleep(GRIP_CLAMP_WAIT_ODD) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -694.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0, 0, -694.816,0,0,0] (lift with book placed on gripper) successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY > pallet_length / 2: # 아래 팔레트 진입 자세 robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose for odd layer grid 2 and 4 successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 1: # grid 2 robot.joint_move_extend([radians(-83.418), radians(103.042), radians(61.470), radians(105.488), radians(-90), radians(-85.718)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: iron_wheel_pallet(odd layer) grid 2 Ready pose/ joint_move_extend [radians(-83.418), radians(103.042), radians(61.470), radians(105.488), radians(-90), radians(-85.718)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 31, centerY + 6, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2 : [{centerX + 31}, {centerY + 6}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) if current_data_index == 3: # grid 4 robot.joint_move_extend([radians(-66.765), radians(105.573), radians(58.163), radians(106.263), radians(-90), radians(-69.065)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: iron_wheel_pallet(odd layer) grid 4 Ready pose/ joint_move_extend [radians(-66.765), radians(105.573), radians(58.163), radians(106.263), radians(-90), radians(-69.065)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 24, centerY + 6, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 4 : [{centerX + 24}, {centerY + 6}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) else: # 위에 팔레트 진입 자세 robot.joint_move_extend(lower_pallet_enter_pose1, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose1 for odd layer grid 1 and 3 successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # grid 1 robot.joint_move_extend([radians(-84.588), radians(122.643), radians(32.227), radians(115.130), radians(-90), radians(-86.888)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: iron_wheel_pallet(odd layer) grid 1 Ready pose/ joint_move_extend [radians(-84.588), radians(122.643), radians(32.227), radians(115.130), radians(-90), radians(-86.888)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 31, centerY + 2, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 31}, {centerY + 2}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) if current_data_index == 2: # grid 3 robot.joint_move_extend([radians(-70.720), radians(126.085), radians(26.197), radians(117.718), radians(-90), radians(-73.020)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: iron_wheel_pallet(odd layer) grid 3 Ready pose/ joint_move_extend [radians(-70.720), radians(126.085), radians(26.197), radians(117.718), radians(-90), radians(-73.020)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 24, centerY + 2, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3 : [{centerX + 24}, {centerY + 2}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) current_layer_height = z_pos # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - iron_wheel_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - iron_wheel_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -180, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [적재] 포크만 위로 -180mm 인출 write_log(f"BE: linear_move_extend : [0, 0, -180, 0, 0, 0] (move robot little bit up) successfully completed - iron_wheel_pallet") is_in_pose() time.sleep(FORK_EXIT_WAIT) # (기존 0.5s) 책 안정화 후 푸셔 해제 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - iron_wheel_pallet") if (EMO(1, 0)): continue robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(1, 0)): continue current_data_index += 1 cache_data['currentDataIndex'] = current_data_index # @@@ current_layer_height는 모든 동작이 무사히 완료된 후에 json에 저장하는 걸로..? " cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) is_move_pose = True else: # 짝수 번째 층(2,4,6...): 그리퍼를 돌려 반대 방향 픽업(맞물림 적재로 하중 분산) write_log("BE: load book on pallet's else condition started - iron_wheel_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - iron_wheel_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - iron_wheel_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([194.344, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (짝수층: 반대 방향에서 진입) write_log("BE: linear_move_extend : [194.344,0,0,0,0,0] (move toward stacker to grip book) successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - iron_wheel_pallet") time.sleep(GRIP_CLAMP_WAIT_EVEN) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -703.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0,0,-703.816,0,0,0] (lift with book placed on gripper) successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY > pallet_length /2: robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 5, 1, 0.1) # 짝수층은 파지 자세 특성상 저속 진입(책 미끄러짐 방지). 미끄러지면 speed/acc를 더 낮출 것 write_log("BE: joint_move_extend : lower_pallet_enter_pose for even layer gird 2 and 4 successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 1: # grid 2 robot.joint_move_extend([radians(-83.418), radians(103.042), radians(61.470), radians(105.488), radians(-90), radians(-85.718)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: iron_wheel_pallet(even layer) grid 2 Ready pose/ joint_move_extend [radians(-83.418), radians(103.042), radians(61.470), radians(105.488), radians(-90), radians(-85.718)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 31, centerY + 6, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2 : [{centerX + 31}, {centerY + 6}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) if current_data_index == 3: # grid 4 robot.joint_move_extend([radians(-66.765), radians(105.573), radians(58.163), radians(106.263), radians(-90), radians(-69.065)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: iron_wheel_pallet(even layer) grid 4 Ready pose/ joint_move_extend [radians(-66.765), radians(105.573), radians(58.163), radians(106.263), radians(-90), radians(-69.065)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 24, centerY + 6, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 4: [{centerX + 24}, {centerY + 6}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) else: robot.joint_move_extend(lower_pallet_enter_pose1, 0, False, 5, 1, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose1 for even layer gird 1 and 3 successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # grid 1 robot.joint_move_extend([radians(-84.588), radians(122.643), radians(32.227), radians(115.130), radians(-90), radians(-86.888)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: iron_wheel_pallet(even layer) grid 1 Ready pose/ joint_move_extend [radians(-84.588), radians(122.643), radians(32.227), radians(115.130), radians(-90), radians(-86.888)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 31, centerY + 2, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 31}, {centerY + 2}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) if current_data_index == 2: # grid 3 robot.joint_move_extend([radians(-70.720), radians(126.085), radians(26.197), radians(117.718), radians(-90), radians(-73.020)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: iron_wheel_pallet(even layer) grid 3 Ready pose/ joint_move_extend [radians(-70.720), radians(126.085), radians(26.197), radians(117.718), radians(-90), radians(-73.020)]") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([centerX + 24, centerY + 2, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3 : [{centerX + 24}, {centerY + 2}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) # @@@@@@@@@@@@@@@@@@@current_layer_height 에 z값 저장 (z값은 is_in_pose 함수에서 가져옴, )@@@@@@@@@@@@@@@@@@@@@ current_layer_height = z_pos # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - iron_wheel_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - iron_wheel_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -180, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [적재] 포크만 위로 -180mm 인출 write_log(f"BE: linear_move_extend : [0,0, -180, 0,0, 0] (move robot little bit up) successfully completed - iron_wheel_pallet") is_in_pose() time.sleep(FORK_EXIT_WAIT) # (기존 0.5s) 책 안정화 후 푸셔 해제 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - iron_wheel_pallet") if (EMO(1, 0)): continue robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - iron_wheel_pallet") is_in_pose() if (EMO(1, 0)): continue current_data_index += 1 cache_data['currentDataIndex'] = current_data_index # @@@ current_layer_height는 모든 동작이 무사히 완료된 후에 json에 저장하는 걸로..? " cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) is_move_pose = True def wooden_Flat_pallet(): """[일반 적재] 나무 납작(낮은) 팔레트: 층당 8권을 그리퍼 -92.3° 회전 자세로 적재. wooden_wheel_pallet() 과 동일한 시퀀스. 차이점: - 층당 8권(그리드 1~8), 적재 가능 높이가 큼(flat_max_current_layer_height=1080) - 책 하중 보정을 위해 set_payload(14~15kg) 호출 - 첫 1~2층은 저속(2500mm/s) 하강으로 안착 안정성 확보 """ write_log("BE: wooden_Flat_pallet function started") global action, immediately_take_book global emergency_stop, protected_stop_stat global power_on_stat, enabled_stat global robot_start_clicked, terminate_robot_button_clicked global pallet_width, pallet_length, pallet_height, book_center_positions global stacker_thread_running, stacker_thread_operator global is_emo_disabled, stacker_vertical_cyl_sensor, stacker_book_ready_sensor, stacker_vertical_cyl_down_sensor # @@@ 새로 추가한 변수 global z_pos global wooden_Flat_pallet_max_z_pos # @@@ is_move_pose = True robot.linear_move_extend([0, 0, -350, 0, 0, 0], 1, True, 7000, 2000, 0.1) write_log("BE: linear_move_extend : [0, 0, -350, 0, 0, 0] successfully completed - wooden_Flat_pallet") home_pos = [radians(-58.423), radians(95.567), radians(66.994), radians(107.440), radians(-90), radians(31.577)] low_profile_1_to_4_enter_pose = [radians(-63.525), radians(133.785), radians(21.038), radians(115.177), radians(-90), radians(-155.138)] low_profile_5_to_7_enter_pose = [radians(-56.413), radians(106.471), radians(65.326), radians(98.204), radians(-90), radians(-148.025)] # 로봇 쪽(5,6,7,8) pallet_8th_enter_pose = [radians(-62.445), radians(103.812), radians(68.820), radians(97.369), radians(-90), radians(-154.057)] robot.login() write_log("BE: login successfully completed - wooden_Flat_pallet") robot.power_on() write_log("BE: power_on successfully completed - wooden_Flat_pallet") robot.enable_robot() write_log("BE: enable_robot successfully completed - wooden_Flat_pallet") robot.joint_move_extend(home_pos, 0, True, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_Flat_pallet") if os.path.exists(cache_json_file_path) and os.path.getsize(cache_json_file_path) > 0: with open(cache_json_file_path, 'r', encoding='utf-8') as file: cache_data = json.load(file) current_data_index_cache = cache_data.get('currentDataIndex') current_layer_index_cache = cache_data.get('currentLayerIndex') current_layer_height_cache = cache_data.get('currentLayerHeight') current_data_index = current_data_index_cache current_layer_index = current_layer_index_cache current_layer_height = current_layer_height_cache else: cache_data = { "currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0 } with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) current_data_index = 0 current_layer_index = 0 current_layer_height = 0 while True: # busy-wait 방지: 짧게 쉬어 상태 폴링 스레드(GIL)가 센서값을 제때 갱신하도록 함 time.sleep(IDLE_LOOP_INTERVAL) if (EMO(0, 0)): is_move_pose = True continue # UI '동작 종료' → 진행 상태 초기화 후 루프 종료 (스태커/컨베이어 정지) if action == 'robot_finish_action': action = None current_data_index = 0 current_layer_index = 0 current_layer_height = 0 terminate_robot_button_clicked = True cache_data['currentLayerIndex'] = current_layer_index cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - wooden_Flat_pallet") write_log("BE: action == 'robot_finish_action' successfully completed - wooden_Flat_pallet") break # 다음 층(130mm)을 쌓으면 최대 높이 초과 → 팔레트 완료 처리 후 종료 if current_layer_height + 130 > flat_max_current_layer_height and current_data_index == 8: action = None current_data_index = 0 current_layer_index = 0 current_layer_height = 0 terminate_robot_button_clicked = True cache_data['currentLayerIndex'] = current_layer_index cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - wooden_Flat_pallet") write_log("BE: current_layer_height > flat_max_current_layer_height successfully completed - wooden_Flat_pallet") break # 8권 다 놓으면 레이어 index + 1 if(current_data_index == 8): current_layer_index += 1 current_data_index = 0 cache_data['currentLayerIndex'] = current_layer_index cache_data['currentDataIndex'] = current_data_index cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) # ── 픽업 대기 자세 잡기 (사이클 완료/비상 복구 후 1회씩 수행) ── # 홈 → 픽업 준비 자세 → 픽업 대기 높이 하강 순서로 이동해 두고 책을 기다린다. if is_move_pose == True and is_emo_disabled == True: is_move_pose = False if current_layer_index % 2 == 0: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_Flat_pallet") is_in_pose() pickup_pose = [radians(-47.102), radians(84.387), radians(78.169), radians(107.444), radians(-90), radians(131.510)] robot.joint_move_extend(pickup_pose, 0, False, 8000, 3000, 0.1) # 책 뜨기 공중 준비 자세 write_log("BE: joint_move_extend : pickup_pose successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 666.816, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(스태커 옆 공중 대기) write_log("BE: linear_move_extend : [0,0,666.816,0,0,0] (ready pose) successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 0)): continue else: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_Flat_pallet") is_in_pose() pickup_pose_opposite = [radians(-24.283), radians(120.475), radians(32.213), radians(117.312), radians(-90), radians(-25.671)] robot.joint_move_extend(pickup_pose_opposite, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : pickup_pose_opposite successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 681.817, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(짝수층 대기 위치) write_log("BE: linear_move_extend : [0,0,681.817,0,0,0] (ready pose) successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 0)): continue # ── 픽업 시작 조건 ── # (스택 픽업량 도달(DI1) AND 리프터 완전 하강(DI3) AND 로봇 정상) # 또는 (UI '즉시 픽업' AND 리프터 상승 상태 아님 AND 로봇 정상) if (((stacker_book_ready_sensor == 1) and (stacker_vertical_cyl_sensor == 0) and (stacker_vertical_cyl_down_sensor == 1)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)) or (((immediately_take_book == 'robot_immediately_take_book') and (stacker_vertical_cyl_sensor == 0)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)): immediately_take_book = None if current_layer_index % 2 == 0: # 홀수 번째 층(1,3,5...): 기본 방향으로 픽업 write_log("BE: load book on pallet's if condition started - wooden_Flat_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - wooden_Flat_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - wooden_Flat_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([-298.306, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (포크가 책 밑으로 들어감) write_log("BE: linear_move_extend : [-298.306, 0, 0,0,0,0] (move toward stacker to grip book) successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - wooden_Flat_pallet") time.sleep(GRIP_CLAMP_WAIT_ODD) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -694.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0, 0, -694.816,0,0,0] (lift with book placed on gripper) successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY < pallet_length / 2: # 4 3 2 1 robot.joint_move_extend(low_profile_1_to_4_enter_pose, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : low_profile_1_to_4_enter_pose for odd layer grid 1,2,3,4 successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # 1번 robot.joint_move_extend([radians(-97.824), radians(132.232), radians(23.979), radians(113.789), radians(-90), radians(-190.124)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(odd layer) grid 1 Ready pose/ joint_move_extend [radians(-84.588), radians(122.643), radians(32.227), radians(115.130), radians(-90), radians(-86.888)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 246, centerY + 155, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 246}, {centerY + 155}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 1: # 2번 robot.joint_move_extend([radians(-88.388), radians(127.129), radians(33.265), radians(109.606), radians(-90), radians(-180.688)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(odd layer) grid 2 Ready pose/ joint_move_extend [radians(-88.388), radians(127.129), radians(33.265), radians(109.606), radians(-90), radians(-180.688)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 240, centerY + 155, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2 : [{centerX + 240}, {centerY + 155}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 2: # 3번 robot.joint_move_extend([radians(-78.851), radians(126.862), radians(33.735), radians(109.403), radians(-90), radians(-171.151)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(odd layer) grid 3 Ready pose/ joint_move_extend [radians(-78.851), radians(126.862), radians(33.735), radians(109.403), radians(-90), radians(-171.151)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 234, centerY + 155, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3 : [{centerX + 234}, {centerY + 155}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 3:# 4번 robot.joint_move_extend([radians(-69.724), radians(131.237), radians(25.835), radians(112.928), radians(-90), radians(-162.024)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(odd layer) grid 4 Ready pose/ joint_move_extend [radians(-69.724), radians(131.237), radians(25.835), radians(112.928), radians(-90), radians(-162.024)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 228, centerY + 155, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 4 : [{centerX + 228}, {centerY + 155}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) else: if current_data_index == 7: #8번 robot.joint_move_extend(pallet_8th_enter_pose, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : pallet_8th_enter_pose(odd layer) successfully completed - wooden_Flat_pallet") is_in_pose() robot.joint_move_extend([radians(-63.913), radians(103.999), radians(68.579), radians(97.422), radians(-90.000), radians(-156.213)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(odd layer) grid 8 Ready pose/ joint_move_extend [radians(-63.913), radians(103.999), radians(68.579), radians(97.422), radians(-90.000), radians(-156.213)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 228, centerY + 159, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 8 : [{centerX + 228}, {centerY + 159}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) else: robot.joint_move_extend(low_profile_5_to_7_enter_pose, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : low_profile_5_to_7_enter_pose for odd layer grid 5,6,7 successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 4: # 5번 robot.joint_move_extend([radians(-100.110), radians(104.625), radians(67.768), radians(97.607), radians(-90), radians(-192.410)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(odd layer) grid 5 Ready pose/ joint_move_extend [radians(-100.110), radians(104.625), radians(67.768), radians(97.607), radians(-90), radians(-192.410)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 246, centerY + 159, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 5 : [{centerX + 246}, {centerY + 159}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 5: # 6번 robot.joint_move_extend([radians(-87.895), radians(101.029), radians(72.323), radians(96.648), radians(-90), radians(-180.195)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(odd layer) grid 6 Ready pose/ joint_move_extend [radians(-87.895), radians(101.029), radians(72.323), radians(96.648), radians(-90), radians(-180.195)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 240, centerY + 159, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 6 : [{centerX + 240}, {centerY + 159}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 6: # 7번 robot.joint_move_extend([radians(-75.459), radians(100.816), radians(72.585), radians(96.599), radians(-90), radians(-167.759)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(odd layer) grid 7 Ready pose/ joint_move_extend [radians(-75.459), radians(100.816), radians(72.585), radians(96.599), radians(-90), radians(-167.759)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 234, centerY + 159, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 7 : [{centerX + 234}, {centerY + 159}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) # @@@@@@@@@@@@@@@@@@@current_layer_height 에 z값 저장 (z값은 is_in_pose 함수에서 가져옴, )@@@@@@@@@@@@@@@@@@@@@ current_layer_height = z_pos robot.set_payload(mass= 14, centroid =[0,0,0]) write_log("Set payload 14") # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - wooden_Flat_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - wooden_Flat_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -180, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [적재] 포크만 위로 -180mm 인출 write_log(f"BE: linear_move_extend : [0, 0, -180, 0, 0, 0] (move robot little bit up) successfully completed - wooden_Flat_pallet") is_in_pose() time.sleep(FORK_EXIT_WAIT) # (기존 0.5s) 책 안정화 후 푸셔 해제 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - wooden_Flat_pallet") if (EMO(1, 0)): continue robot.linear_move_extend([0,0,-(wooden_Flat_pallet_max_z_pos - (current_layer_height)), 0, 0, 0], 1, False, 8000, 3000, 0.1) write_log(f"BE: linear_move_extend : [0, 0, {-(wooden_Flat_pallet_max_z_pos - (current_layer_height))}, 0, 0, 0] successfully completed - wooden_Flat_pallet") is_in_pose() robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(1, 0)): continue current_data_index += 1 cache_data['currentDataIndex'] = current_data_index # @@@ current_layer_height는 모든 동작이 무사히 완료된 후에 json에 저장하는 걸로..? " cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) is_move_pose = True else: # 짝수 번째 층(2,4,6...): 그리퍼를 돌려 반대 방향 픽업(맞물림 적재로 하중 분산) write_log("BE: load book on pallet's else condition started - wooden_Flat_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - wooden_Flat_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - wooden_Flat_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([194.344, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (짝수층: 반대 방향에서 진입) write_log("BE: linear_move_extend : [194.344,0,0,0,0,0] (move toward stacker to grip book) successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - wooden_Flat_pallet") time.sleep(GRIP_CLAMP_WAIT_EVEN) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -703.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0,0,-703.816,0,0,0] (lift with book placed on gripper) successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY < pallet_length /2: robot.joint_move_extend(low_profile_1_to_4_enter_pose, 0, False, 8000, 3000, 0.1) # 짝수층은 파지 자세 특성상 저속 진입(책 미끄러짐 방지). 미끄러지면 speed/acc를 더 낮출 것 write_log("BE: joint_move_extend : low_profile_1_to_4_enter_pose for even layer gird 1,2,3,4 successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # 1번 robot.joint_move_extend([radians(-97.824), radians(132.232), radians(23.979), radians(113.789), radians(-90), radians(-190.124)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(even layer) grid 1 Ready pose/ joint_move_extend [radians(-97.824), radians(132.232), radians(23.979), radians(113.789), radians(-90), radians(-190.124)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 246, centerY + 155, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 246}, {centerY + 155}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 1: # 2번 robot.joint_move_extend([radians(-88.388), radians(127.129), radians(33.265), radians(109.606), radians(-90), radians(-180.688)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(even layer) grid 2 Ready pose/ joint_move_extend [radians(-88.388), radians(127.129), radians(33.265), radians(109.606), radians(-90), radians(-180.688)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 240, centerY + 155, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2 : [{centerX + 240}, {centerY + 155}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 2: # 3번 robot.joint_move_extend([radians(-78.851), radians(126.862), radians(33.735), radians(109.403), radians(-90), radians(-171.151)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(even layer) grid 3 Ready pose/ joint_move_extend [radians(-78.851), radians(126.862), radians(33.735), radians(109.403), radians(-90), radians(-171.151)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 234, centerY + 155, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3 : [{centerX + 234}, {centerY + 155}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 3:# 4번 robot.joint_move_extend([radians(-69.724), radians(131.237), radians(25.835), radians(112.928), radians(-90), radians(-162.024)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(even layer) grid 4 Ready pose/ joint_move_extend [radians(-69.724), radians(131.237), radians(25.835), radians(112.928), radians(-90), radians(-162.024)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 228, centerY + 155, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 4 : [{centerX + 228}, {centerY + 155}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) else: if current_data_index == 7: # GRID 8 robot.joint_move_extend(pallet_8th_enter_pose, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : pallet_8th_enter_pose for even layer successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue robot.joint_move_extend([radians(-63.913), radians(103.999), radians(68.579), radians(97.422), radians(-90.000), radians(-156.213)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(even layer) grid 8 Ready pose/ joint_move_extend [radians(-63.913), radians(103.999), radians(68.579), radians(97.422), radians(-90.000), radians(-156.213)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 228, centerY + 159, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 8 : [{centerX + 228}, {centerY + 159}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) else: robot.joint_move_extend(low_profile_5_to_7_enter_pose, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : low_profile_5_to_7_enter_pose for even layer grid 5,6,7 successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 4: # 5번 robot.joint_move_extend([radians(-100.110), radians(104.625), radians(67.768), radians(97.607), radians(-90), radians(-192.410)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(even layer) grid 5 Ready pose/ joint_move_extend [radians(-100.110), radians(104.625), radians(67.768), radians(97.607), radians(-90), radians(-192.410)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 246, centerY + 159, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 5: [{centerX + 246}, {centerY + 159}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 5: # 6번 robot.joint_move_extend([radians(-87.895), radians(101.029), radians(72.323), radians(96.648), radians(-90), radians(-180.195)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(even layer) grid 6 Ready pose/ joint_move_extend [radians(-87.895), radians(101.029), radians(72.323), radians(96.648), radians(-90), radians(-180.195)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 240, centerY + 159, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 6 : [{centerX + 240}, {centerY + 159}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) if current_data_index == 6: # 7번 robot.joint_move_extend([radians(-75.459), radians(100.816), radians(72.585), radians(96.599), radians(-90), radians(-167.759)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: wooden_Flat_pallet(even layer) grid 7 Ready pose/ joint_move_extend [radians(-75.459), radians(100.816), radians(72.585), radians(96.599), radians(-90), radians(-167.759)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 234, centerY + 159, -wooden_Flat_pallet_max_z_pos, 0, 0, radians(-92.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 7 : [{centerX + 234}, {centerY + 159}, {-wooden_Flat_pallet_max_z_pos}, 0, 0, radians(-92.3)] successfully completed - wooden_Flat_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_Flat_pallet_max_z_pos) # @@@@@@@@@@@@@@@@@@@current_layer_height 에 z값 저장 (z값은 is_in_pose 함수에서 가져옴, )@@@@@@@@@@@@@@@@@@@@@ current_layer_height = z_pos robot.set_payload(mass= 14, centroid =[0,0,0]) write_log("Set payload 14") # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - wooden_Flat_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - wooden_Flat_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -180, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [적재] 포크만 위로 -180mm 인출 write_log(f"BE: linear_move_extend : [0,0, -180, 0, 0, 0] (move robot little bit up) successfully completed - wooden_Flat_pallet") is_in_pose() time.sleep(FORK_EXIT_WAIT) # (기존 0.5s) 책 안정화 후 푸셔 해제 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - wooden_Flat_pallet") if (EMO(1, 0)): continue robot.linear_move_extend([0, 0, -(wooden_Flat_pallet_max_z_pos - (current_layer_height)), 0, 0, 0], 1, False, 8000, 3000, 0.1) write_log(f"BE: linear_move_extend : [0, 0, {-(wooden_Flat_pallet_max_z_pos - (current_layer_height))}, 0, 0, 0] successfully completed - wooden_Flat_pallet") is_in_pose() robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - wooden_Flat_pallet") is_in_pose() if (EMO(1, 0)): continue current_data_index += 1 cache_data['currentDataIndex'] = current_data_index # @@@ current_layer_height는 모든 동작이 무사히 완료된 후에 json에 저장하는 걸로..? " cache_data['currentLayerHeight'] = current_layer_height with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) is_move_pose = True def one_line_wooden_wheel_pallet(): """[한줄쌓기] 나무 바퀴 팔레트: 선택한 그리드 한 곳에만 수직으로 계속 쌓는 모드. 일반 적재와 달리 UI에서 고른 grid_id 위치에만 적재한다. - currentDataIndex 는 그리드 번호 고정값(0~3)이며 증가하지 않는다. - 층수/높이 진행 상태는 oneLine.json 에 그리드별로 저장한다. - 모든 그리드가 최대 높이에 도달하면 자동 종료된다. """ write_log("BE: one_line_wooden_wheel_pallet function started") global action, immediately_take_book global emergency_stop, protected_stop_stat global power_on_stat, enabled_stat global robot_start_clicked, terminate_robot_button_clicked global pallet_width, pallet_length, pallet_height, book_center_positions global stacker_thread_running, stacker_thread_operator global is_one_line_stop_clicked global is_emo_disabled, stacker_vertical_cyl_sensor, stacker_book_ready_sensor, stacker_vertical_cyl_down_sensor # @@@ 새로 추가한 변수 global z_pos global wooden_wheel_pallet_max_z_pos # @@@ is_move_pose = True robot.linear_move_extend([0, 0, -350, 0, 0, 0], 1, True, 7000, 2000, 0.1) write_log("BE: linear_move_extend : [0, 0, -350, 0, 0, 0] successfully completed - one_line_wooden_wheel_pallet") home_pos = [radians(-58.423), radians(95.567), radians(66.994), radians(107.440), radians(-90), radians(31.577)] lower_pallet_enter_pose = [radians(-61.533), radians(107.484), radians(55.576), radians(106.940), radians(-90), radians(-61.533)] lower_pallet_enter_pose1 = [radians(-66.797), radians(136.143), radians(6.799), radians(127.058), radians(-90), radians(-66.797)] robot.login() write_log("BE: login successfully completed - one_line_wooden_wheel_pallet") robot.power_on() write_log("BE: power_on successfully completed - one_line_wooden_wheel_pallet") robot.enable_robot() write_log("BE: enable_robot successfully completed - one_line_wooden_wheel_pallet") robot.joint_move_extend(home_pos, 0, True, 10, 1, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_wooden_wheel_pallet") if os.path.exists(oneLine_json_file_path) and os.path.getsize(oneLine_json_file_path) > 0: with open(oneLine_json_file_path, 'r', encoding='utf-8') as file: oneLine_data = json.load(file) grid_data = oneLine_data.get(grid_id, {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}) current_data_index = grid_data.get('currentDataIndex', 0) current_layer_index = grid_data.get('currentLayerIndex', 0) current_layer_height = grid_data.get('currentLayerHeight', 0) else: oneLine_data = { "grid1": {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid2": {"currentDataIndex": 1, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid3": {"currentDataIndex": 2, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid4": {"currentDataIndex": 3, "currentLayerIndex": 0, "currentLayerHeight": 0} } with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) current_data_index = 0 current_layer_index = 0 current_layer_height = 0 while True: # busy-wait 방지: 짧게 쉬어 상태 폴링 스레드(GIL)가 센서값을 제때 갱신하도록 함 time.sleep(IDLE_LOOP_INTERVAL) if (EMO(0, 0)): is_move_pose = True continue # 로봇 동작 종료 누르면 초기화 및 while 탈출 / 스태커 멈춤 # UI '동작 종료' → 진행 상태 초기화 후 루프 종료 (스태커/컨베이어 정지) if action == 'robot_finish_action': action = None current_layer_index = 0 current_layer_height = 0 terminate_robot_button_clicked = True oneLine_data = { "grid1": {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid2": {"currentDataIndex": 1, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid3": {"currentDataIndex": 2, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid4": {"currentDataIndex": 3, "currentLayerIndex": 0, "currentLayerHeight": 0} } with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() # Wait for the thread to terminate robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - one_line_wooden_wheel_pallet") write_log("BE: action == 'robot_finish_action' successfully completed - one_line_wooden_wheel_pallet") break # 모든 그리드가 다 6단을 쌓으면 로봇 동작 종료 if all(oneLine_data[grid]['currentLayerHeight'] > oneline_wheel_max_current_layer_height for grid in oneLine_data): action = 'robot_finish_action' write_log("BE: all(oneLine_data[grid]['currentLayerHeight'] > oneline_wheel_max_current_layer_height for grid in oneLine_data) successfully completed - one_line_wooden_wheel_pallet") continue # 한줄쌓기멈춤 버튼을 누르면 while 탈출 / 스태커 멈춤 if is_one_line_stop_clicked == 'one_line_stop': is_one_line_stop_clicked = None terminate_robot_button_clicked = True stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - one_line_wooden_wheel_pallet") write_log("BE: is_one_line_stop_clicked == 'one_line_stop' successfully completed - one_line_wooden_wheel_pallet") break # 만약 6단(최고높이)을 쌓으면 while 탈출 / 스태커 멈춤 if current_layer_height + 130 > oneline_wheel_max_current_layer_height: action = None terminate_robot_button_clicked = True stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - one_line_wooden_wheel_pallet") write_log("BE: current_layer_height > oneline_wheel_max_current_layer_height successfully completed - one_line_wooden_wheel_pallet") break # ── 픽업 대기 자세 잡기 (사이클 완료/비상 복구 후 1회씩 수행) ── # 홈 → 픽업 준비 자세 → 픽업 대기 높이 하강 순서로 이동해 두고 책을 기다린다. if is_move_pose == True and is_emo_disabled == True: is_move_pose = False if current_layer_index % 2 == 0: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_wooden_wheel_pallet") is_in_pose() pickup_pose = [radians(-47.102), radians(84.387), radians(78.169), radians(107.444), radians(-90), radians(131.510)] robot.joint_move_extend(pickup_pose, 0, False, 8000, 3000, 0.1) # 책 뜨기 공중 준비 자세 write_log("BE: joint_move_extend : pickup_pose successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 666.816, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(스태커 옆 공중 대기) write_log("BE: linear_move_extend : [0,0,666.816,0,0,0] (reday pose) successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue else: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_wooden_wheel_pallet") is_in_pose() pickup_pose_opposite = [radians(-24.283), radians(120.475), radians(32.213), radians(117.312), radians(-90), radians(-25.671)] robot.joint_move_extend(pickup_pose_opposite, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : pickup_pose_opposite successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 681.817, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(짝수층 대기 위치) write_log("BE: linear_move_extend : [0,0,681.817,0,0,0] (ready pose) successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue # ── 픽업 시작 조건 ── # (스택 픽업량 도달(DI1) AND 리프터 완전 하강(DI3) AND 로봇 정상) # 또는 (UI '즉시 픽업' AND 리프터 상승 상태 아님 AND 로봇 정상) if (((stacker_book_ready_sensor == 1) and (stacker_vertical_cyl_sensor == 0) and (stacker_vertical_cyl_down_sensor == 1)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)) or (((immediately_take_book == 'robot_immediately_take_book') and (stacker_vertical_cyl_sensor == 0)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)): immediately_take_book = None if current_layer_index % 2 == 0: # 홀수 번째 층(1,3,5...): 기본 방향으로 픽업 write_log("BE: load book on pallet's if condition started - one_line_wooden_wheel_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - one_line_wooden_wheel_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - one_line_wooden_wheel_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([-298.306, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (포크가 책 밑으로 들어감) write_log("BE: linear_move_extend : [-298.306, 0, 0,0,0,0] (move toward stacker to grip book) successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - one_line_wooden_wheel_pallet") time.sleep(GRIP_CLAMP_WAIT_ODD) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -694.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0, 0, -694.816,0,0,0] (lift with book placed on gripper) successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY > pallet_length / 2: # 아래 팔레트 진입 자세 robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose for odd layer grid 2 and 4 successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 1: # 2번 grid robot.joint_move_extend([radians(-83.398), radians(102.810), radians(61.767), radians(105.424), radians(-90), radians(-85.698)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_wooden_wheel_pallet(odd layer) grid 2 Ready pose/ joint_move_extend [radians(-83.398), radians(102.810), radians(61.767), radians(105.424), radians(-90), radians(-85.698)]") is_in_pose() if (EMO(0,0)): continue robot.linear_move_extend([centerX + 31, centerY + 9, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2 : [{centerX + 31}, {centerY + 9}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0,0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) if current_data_index == 3: # 4번 grid robot.joint_move_extend([radians(-66.699), radians(105.342), radians(58.472), radians(106.187), radians(-90), radians(-68.999)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_wooden_wheel_pallet(odd layer) grid 4 Ready pose/ joint_move_extend [radians(-66.699), radians(105.342), radians(58.472), radians(106.187), radians(-90), radians(-68.999)]") is_in_pose() if (EMO(0,0)): continue robot.linear_move_extend([centerX + 24, centerY + 9, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 4 : [{centerX + 24}, {centerY + 9}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0,0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) else: # 위에 팔레트 진입 자세 robot.joint_move_extend(lower_pallet_enter_pose1, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose1 for odd layer grid 1 and 3 successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # grid 1번 robot.joint_move_extend([radians(-84.575), radians(122.286), radians(32.836), radians(114.878), radians(-90), radians(-86.875)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_wooden_wheel_pallet(odd layer) grid 1 Ready pose/ joint_move_extend [radians(-84.575), radians(122.286), radians(32.836), radians(114.878), radians(-90), radians(-86.875)]") is_in_pose() if (EMO(0,0)): continue robot.linear_move_extend([centerX + 31, centerY + 5, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 31}, {centerY + 5}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0,0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) if current_data_index == 2: # grid 3번 robot.joint_move_extend([radians(-70.674), radians(125.675), radians(26.930), radians(117.394), radians(-90), radians(-72.974)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_wooden_wheel_pallet(odd layer) grid 3 Ready pose/ joint_move_extend [radians(-70.674), radians(125.675), radians(26.930), radians(117.394), radians(-90), radians(-72.974)]") is_in_pose() if (EMO(0,0)): continue robot.linear_move_extend([centerX + 24, centerY + 5, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3 : [{centerX + 24}, {centerY + 5}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0,0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) # @@@@@@@@@@@@@@@@@@@current_layer_height 에 z값 저장 (z값은 is_in_pose 함수에서 가져옴, )@@@@@@@@@@@@@@@@@@@@@ current_layer_height = z_pos # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - one_line_wooden_wheel_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - one_line_wooden_wheel_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -(wooden_wheel_pallet_max_z_pos - (current_layer_height)), 0, 0, 0], 1, False, 8000, 3000, 0.1) write_log(f"BE:linear_move_extend : [0, 0, {-(wooden_wheel_pallet_max_z_pos - (current_layer_height))}, 0, 0, 0] (move robot little bit up) successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - one_line_wooden_wheel_pallet") if (EMO(1, 0)): continue robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(1, 0)): continue current_layer_index += 1 oneLine_data[grid_id]['currentLayerIndex'] = current_layer_index oneLine_data[grid_id]['currentLayerHeight'] = current_layer_height with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) is_move_pose = True else: # 짝수 번째 층(2,4,6...): 그리퍼를 돌려 반대 방향 픽업(맞물림 적재로 하중 분산) write_log("BE: load book on pallet's else condition started - one_line_wooden_wheel_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - one_line_wooden_wheel_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - one_line_wooden_wheel_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([194.344, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (짝수층: 반대 방향에서 진입) write_log("BE: linear_move_extend : [194.344,0,0,0,0,0] (move toward stacker to grip book) successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - one_line_wooden_wheel_pallet") time.sleep(GRIP_CLAMP_WAIT_EVEN) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -703.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0,0,-703.816,0,0,0] (lift with book placed on gripper) successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY > pallet_length /2: robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 5, 1, 0.1) # 짝수층은 파지 자세 특성상 저속 진입(책 미끄러짐 방지). 미끄러지면 speed/acc를 더 낮출 것 write_log("BE: joint_move_extend : lower_pallet_enter_pose for even layer 2 and 4 successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 1: # grid 2번 robot.joint_move_extend([radians(-83.398), radians(102.810), radians(61.767), radians(105.424), radians(-90), radians(-85.698)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_wooden_wheel_pallet(even layer) grid 2 Ready pose/ joint_move_extend [radians(-83.398), radians(102.810), radians(61.767), radians(105.424), radians(-90), radians(-85.698)]") is_in_pose() if (EMO(0,0)): continue robot.linear_move_extend([centerX + 31, centerY + 9, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2 : [{centerX + 31}, {centerY + 9}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0,0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) if current_data_index == 3: # grid 4번 robot.joint_move_extend([radians(-66.699), radians(105.342), radians(58.472), radians(106.187), radians(-90), radians(-68.999)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_wooden_wheel_pallet(even layer) grid 4 Ready pose/ joint_move_extend [radians(-66.699), radians(105.342), radians(58.472), radians(106.187), radians(-90), radians(-68.999)]") is_in_pose() if (EMO(0,0)): continue robot.linear_move_extend([centerX + 24, centerY + 9, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 4 : [{centerX + 24}, {centerY + 9}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0,0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) else: robot.joint_move_extend(lower_pallet_enter_pose1, 0, False, 5, 1, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose1 for even layer grid 1 and 3 successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # grid 1번 robot.joint_move_extend([radians(-84.575), radians(122.286), radians(32.836), radians(114.878), radians(-90), radians(-86.875)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_wooden_wheel_pallet(even layer) grid 1 Ready pose/ joint_move_extend [radians(-84.575), radians(122.286), radians(32.836), radians(114.878), radians(-90), radians(-86.875)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 31, centerY + 5, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 31}, {centerY + 5}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) if current_data_index == 2: # grid 3번 robot.joint_move_extend([radians(-70.674), radians(125.675), radians(26.930), radians(117.394), radians(-90), radians(-72.974)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_wooden_wheel_pallet(even layer) grid 3 Ready pose/ joint_move_extend [radians(-70.674), radians(125.675), radians(26.930), radians(117.394), radians(-90), radians(-72.974)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 24, centerY + 5, -wooden_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3 : [{centerX + 24}, {centerY + 5}, {-wooden_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0,0)): continue get_distance_to_base(wooden_wheel_pallet_max_z_pos) # @@@@@@@@@@@@@@@@@@@current_layer_height 에 z값 저장 (z값은 is_in_pose 함수에서 가져옴, )@@@@@@@@@@@@@@@@@@@@@ current_layer_height = z_pos # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - one_line_wooden_wheel_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - one_line_wooden_wheel_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -(wooden_wheel_pallet_max_z_pos - (current_layer_height)), 0, 0, 0], 1, False, 8000, 3000, 0.1) write_log(f"BE:linear_move_extend : [0, 0, {-(wooden_wheel_pallet_max_z_pos - (current_layer_height))}, 0, 0, 0] (move robot little bit up) successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - one_line_wooden_wheel_pallet") if (EMO(1, 0)): continue robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_wooden_wheel_pallet") is_in_pose() if (EMO(1, 0)): continue current_layer_index += 1 oneLine_data[grid_id]['currentLayerHeight'] = current_layer_height oneLine_data[grid_id]['currentLayerIndex'] = current_layer_index with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) is_move_pose = True def one_line_iron_wheel_pallet(): """[한줄쌓기] 철제 바퀴 팔레트: one_line_wooden_wheel_pallet() 과 동일 시퀀스. 차이점: 최대 하강 z(884)와 그리드 진입 자세/좌표 오프셋이 철제 팔레트 기준. """ write_log("BE: one_line_iron_wheel_pallet function started") global action, immediately_take_book global emergency_stop, protected_stop_stat global power_on_stat, enabled_stat global robot_start_clicked, terminate_robot_button_clicked global pallet_width, pallet_length, pallet_height, book_center_positions global stacker_thread_running, stacker_thread_operator global is_one_line_stop_clicked global is_emo_disabled, stacker_vertical_cyl_sensor, stacker_book_ready_sensor, stacker_vertical_cyl_down_sensor # @@@ 새로 추가한 변수 global z_pos global iron_wheel_pallet_max_z_pos # @@@ is_move_pose = True robot.linear_move_extend([0, 0, -350, 0, 0, 0], 1, True, 7000, 2000, 0.1) write_log("BE: linear_move_extend : [0, 0, -350, 0, 0, 0] successfully completed - one_line_iron_wheel_pallet") home_pos = [radians(-58.423), radians(95.567), radians(66.994), radians(107.440), radians(-90), radians(31.577)] lower_pallet_enter_pose = [radians(-61.533), radians(107.484), radians(55.576), radians(106.940), radians(-90), radians(-61.533)] lower_pallet_enter_pose1 = [radians(-66.797), radians(136.143), radians(6.799), radians(127.058), radians(-90), radians(-66.797)] robot.login() write_log("BE: login successfully completed - one_line_iron_wheel_pallet") robot.power_on() write_log("BE: power_on successfully completed - one_line_iron_wheel_pallet") robot.enable_robot() write_log("BE: enable_robot successfully completed - one_line_iron_wheel_pallet") robot.joint_move_extend(home_pos, 0, True, 10, 1, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_iron_wheel_pallet") if os.path.exists(oneLine_json_file_path) and os.path.getsize(oneLine_json_file_path) > 0: with open(oneLine_json_file_path, 'r', encoding='utf-8') as file: oneLine_data = json.load(file) grid_data = oneLine_data.get(grid_id, {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}) current_data_index = grid_data.get('currentDataIndex', 0) current_layer_index = grid_data.get('currentLayerIndex', 0) current_layer_height = grid_data.get('currentLayerHeight', 0) else: oneLine_data = { "grid1": {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid2": {"currentDataIndex": 1, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid3": {"currentDataIndex": 2, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid4": {"currentDataIndex": 3, "currentLayerIndex": 0, "currentLayerHeight": 0} } with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) current_data_index = 0 current_layer_index = 0 current_layer_height = 0 while True: # busy-wait 방지: 짧게 쉬어 상태 폴링 스레드(GIL)가 센서값을 제때 갱신하도록 함 time.sleep(IDLE_LOOP_INTERVAL) if (EMO(0, 0)): is_move_pose = True continue # UI '동작 종료' → 진행 상태 초기화 후 루프 종료 (스태커/컨베이어 정지) if action == 'robot_finish_action': action = None current_layer_index = 0 current_layer_height = 0 terminate_robot_button_clicked = True oneLine_data = { "grid1": {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid2": {"currentDataIndex": 1, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid3": {"currentDataIndex": 2, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid4": {"currentDataIndex": 3, "currentLayerIndex": 0, "currentLayerHeight": 0} } with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - one_line_iron_wheel_pallet") write_log("BE: action == 'robot_finish_action' successfully completed - one_line_iron_wheel_pallet") break if all(oneLine_data[grid]['currentLayerHeight'] > oneline_wheel_max_current_layer_height for grid in oneLine_data): action = 'robot_finish_action' write_log("BE: all(oneLine_data[grid]['currentLayerHeight'] > oneline_wheel_max_current_layer_height for grid in oneLine_data) successfully completed - one_line_iron_wheel_pallet") continue if is_one_line_stop_clicked == 'one_line_stop': is_one_line_stop_clicked = None terminate_robot_button_clicked = True stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - one_line_iron_wheel_pallet") write_log("BE: is_one_line_stop_clicked == 'one_line_stop' successfully completed - one_line_iron_wheel_pallet") break if current_layer_height + 130 > oneline_wheel_max_current_layer_height: action = None terminate_robot_button_clicked = True stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) (conveyor belt off) successfully completed - one_line_iron_wheel_pallet") write_log("BE: current_layer_height > oneline_wheel_max_current_layer_height successfully completed - one_line_iron_wheel_pallet") break # ── 픽업 대기 자세 잡기 (사이클 완료/비상 복구 후 1회씩 수행) ── # 홈 → 픽업 준비 자세 → 픽업 대기 높이 하강 순서로 이동해 두고 책을 기다린다. if is_move_pose == True and is_emo_disabled == True: is_move_pose = False if current_layer_index % 2 == 0: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_iron_wheel_pallet") is_in_pose() pickup_pose = [radians(-47.102), radians(84.387), radians(78.169), radians(107.444), radians(-90), radians(131.510)] robot.joint_move_extend(pickup_pose, 0, False, 8000, 3000, 0.1) # 책 뜨기 공중 준비 자세 write_log("BE: joint_move_extend : pickup_pose successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 666.816, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(스태커 옆 공중 대기) write_log("BE: linear_move_extend : [0,0,666.816,0,0,0] (ready pose) successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue else: robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_iron_wheel_pallet") is_in_pose() pickup_pose_opposite = [radians(-24.283), radians(120.475), radians(32.213), radians(117.312), radians(-90), radians(-25.671)] robot.joint_move_extend(pickup_pose_opposite, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : pickup_pose_opposite successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, 681.817, 0, 0, 0], 1, False, 8000, 3000, 0.1) # 픽업 대기 높이까지 하강(짝수층 대기 위치) write_log("BE: linear_move_extend : [0,0,681.817,0,0,0] (ready pose) successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue # ── 픽업 시작 조건 ── # (스택 픽업량 도달(DI1) AND 리프터 완전 하강(DI3) AND 로봇 정상) # 또는 (UI '즉시 픽업' AND 리프터 상승 상태 아님 AND 로봇 정상) if (((stacker_book_ready_sensor == 1) and (stacker_vertical_cyl_sensor == 0) and (stacker_vertical_cyl_down_sensor == 1)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)) or (((immediately_take_book == 'robot_immediately_take_book') and (stacker_vertical_cyl_sensor == 0)) and (emergency_stop == 0 or protected_stop_stat == 0) and (power_on_stat == 1 and enabled_stat == 1)): immediately_take_book = None if current_layer_index % 2 == 0: # 홀수 번째 층(1,3,5...): 기본 방향으로 픽업 write_log("BE: load book on pallet's if condition started - one_line_iron_wheel_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - one_line_iron_wheel_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - one_line_iron_wheel_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([-298.306, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (포크가 책 밑으로 들어감) write_log("BE: linear_move_extend : [-298.306, 0, 0,0,0,0] (move toward stacker to grip book) successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - one_line_iron_wheel_pallet") time.sleep(GRIP_CLAMP_WAIT_ODD) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -694.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0, 0, -694.816,0,0,0] (lift with book placed on gripper) successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY > pallet_length / 2: # 아래 팔레트 진입 자세 robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose for odd layer grid 2 and 4 successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 1: # grid 2번 robot.joint_move_extend([radians(-83.418), radians(103.042), radians(61.470), radians(105.488), radians(-90), radians(-85.718)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_iron_wheel_pallet(odd layer) grid 2 Ready pose/ joint_move_extend [radians(-83.418), radians(103.042), radians(61.470), radians(105.488), radians(-90), radians(-85.718)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 31, centerY + 6, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2 : [{centerX + 31}, {centerY + 6}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_iron_wheel_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) if current_data_index == 3: # grid 4번 robot.joint_move_extend([radians(-66.765), radians(105.573), radians(58.163), radians(106.263), radians(-90), radians(-69.065)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_iron_wheel_pallet(odd layer) grid 3 Ready pose/ joint_move_extend [radians(-66.765), radians(105.573), radians(58.163), radians(106.263), radians(-90), radians(-69.065)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 24, centerY + 6, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 4 : [{centerX + 24}, {centerY + 6}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_iron_wheel_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) else: # 위에 팔레트 진입 자세 robot.joint_move_extend(lower_pallet_enter_pose1, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose1 odd layer grid 1 and 3 successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # grid 1번 robot.joint_move_extend([radians(-84.588), radians(122.643), radians(32.227), radians(115.130), radians(-90), radians(-86.888)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_iron_wheel_pallet(odd layer) grid 1 Ready pose/ joint_move_extend [radians(-84.588), radians(122.643), radians(32.227), radians(115.130), radians(-90), radians(-86.888)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 31, centerY + 2, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 31}, {centerY + 2}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_iron_wheel_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) if current_data_index == 2: # grid 3번 robot.joint_move_extend([radians(-70.720), radians(126.085), radians(26.197), radians(117.718), radians(-90), radians(-73.020)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_iron_wheel_pallet(odd layer) grid 3 Ready pose/ joint_move_extend [radians(-70.720), radians(126.085), radians(26.197), radians(117.718), radians(-90), radians(-73.020)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 24, centerY + 2, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3 : [{centerX + 24}, {centerY + 2}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_iron_wheel_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) # @@@@@@@@@@@@@@@@@@@current_layer_height 에 z값 저장 (z값은 is_in_pose 함수에서 가져옴, )@@@@@@@@@@@@@@@@@@@@@ current_layer_height = z_pos # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - one_line_iron_wheel_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - one_line_iron_wheel_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -(iron_wheel_pallet_max_z_pos - (current_layer_height)), 0, 0, 0], 1, False, 8000, 3000, 0.1) write_log(f"BE:linear_move_extend : [0, 0, {-(iron_wheel_pallet_max_z_pos - (current_layer_height))}, 0, 0, 0] (move robot little bit up) successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - one_line_iron_wheel_pallet") if (EMO(1, 0)): continue robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(1, 0)): continue current_layer_index += 1 oneLine_data[grid_id]['currentLayerIndex'] = current_layer_index oneLine_data[grid_id]['currentLayerHeight'] = current_layer_height with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) is_move_pose = True else: # 짝수 번째 층(2,4,6...): 그리퍼를 돌려 반대 방향 픽업(맞물림 적재로 하중 분산) write_log("BE: load book on pallet's else condition started - one_line_iron_wheel_pallet") current_data_list = list(book_center_positions) current_data = current_data_list[current_data_index] centerX = current_data['x'] centerY = current_data['y'] # ── [픽업 시퀀스 시작] 그리퍼 2개(수평/수직) OFF 재확인 → 후퇴 대기 → 스태커 진입 ── robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: set_digital_output(IO_CABINET, 1, 0) (for safety purposes : parallel gripper cylinder off) successfully completed - one_line_iron_wheel_pallet") robot.set_digital_output(IO_CABINET, 0, 0) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (for safety purposes : vertical gripper cylinder off) successfully completed - one_line_iron_wheel_pallet") time.sleep(GRIPPER_RETRACT_WAIT) # (기존 save_time=2.0s) 실린더 후퇴 확인용 대기 robot.linear_move_extend([194.344, 0, 0, 0, 0, 0], 1, False, 8000, 3000, 0.1) # [픽업] 스태커로 수평 진입 (짝수층: 반대 방향에서 진입) write_log("BE: linear_move_extend : [194.344,0,0,0,0,0] (move toward stacker to grip book) successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 0, 1) # [픽업] 수직 그리퍼 ON → 책 파지 write_log("BE: set_digital_output(IO_CABINET, 0, 1) (turn ON vertical gripper cylinder to grip book) successfully completed - one_line_iron_wheel_pallet") time.sleep(GRIP_CLAMP_WAIT_EVEN) # 파지(클램프) 완료 대기 if (EMO(0, 0)): continue robot.linear_move_extend([0, 0, -703.816, 0, 0, 0], 1, False, speed, acc, 0.1) # [픽업] 책을 뜬 채 팔레트 상공 높이까지 상승 write_log("BE: linear_move_extend : [0,0,-703.816,0,0,0] (lift with book placed on gripper) successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue time.sleep(BOOK_SETTLE_WAIT) # 책 흔들림 안정화 if centerY > pallet_length /2: robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 5, 1, 0.1) # 짝수층은 파지 자세 특성상 저속 진입(책 미끄러짐 방지). 미끄러지면 speed/acc를 더 낮출 것 write_log("BE: joint_move_extend : lower_pallet_enter_pose for even layer grid 2 and 4 successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 1: # grid 2번 robot.joint_move_extend([radians(-83.418), radians(103.042), radians(61.470), radians(105.488), radians(-90), radians(-85.718)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_iron_wheel_pallet(even layer) grid 2 Ready pose/ joint_move_extend [radians(-83.418), radians(103.042), radians(61.470), radians(105.488), radians(-90), radians(-85.718)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 31, centerY + 6, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 2 : [{centerX + 31}, {centerY + 6}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_iron_wheel_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) if current_data_index == 3: # grid 4번 robot.joint_move_extend([radians(-66.765), radians(105.573), radians(58.163), radians(106.263), radians(-90), radians(-69.065)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_iron_wheel_pallet(even layer) grid 4 Ready pose/ joint_move_extend [radians(-66.765), radians(105.573), radians(58.163), radians(106.263), radians(-90), radians(-69.065)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 24, centerY + 6, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 4 : [{centerX + 24}, {centerY + 6}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_iron_wheel_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) else: robot.joint_move_extend(lower_pallet_enter_pose1, 0, False, 5, 1, 0.1) write_log("BE: joint_move_extend : lower_pallet_enter_pose1 for even layer 1 and 3 successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 1)): continue if current_data_index == 0: # grid 1번 robot.joint_move_extend([radians(-84.588), radians(122.643), radians(32.227), radians(115.130), radians(-90), radians(-86.888)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_iron_wheel_pallet(even layer) grid 1 Ready pose/ joint_move_extend [radians(-84.588), radians(122.643), radians(32.227), radians(115.130), radians(-90), radians(-86.888)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 31, centerY + 2, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 1 : [{centerX + 31}, {centerY + 2}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_iron_wheel_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) if current_data_index == 2: # grid 3번 robot.joint_move_extend([radians(-70.720), radians(126.085), radians(26.197), radians(117.718), radians(-90), radians(-73.020)], 0, False, grid_speed, grid_acc, 0.1) write_log("BE: one_line_iron_wheel_pallet(even layer) grid 3 Ready pose/ joint_move_extend [radians(-70.720), radians(126.085), radians(26.197), radians(117.718), radians(-90), radians(-73.020)]") is_in_pose() if(EMO(0,0)): continue robot.linear_move_extend([centerX + 24, centerY + 2, -iron_wheel_pallet_max_z_pos, 0, 0, radians(-2.3)], 0, False, speed, acc, 0.1) write_log(f"BE: linear_move_extend grid 3 : [{centerX + 24}, {centerY + 2}, {-iron_wheel_pallet_max_z_pos}, 0, 0, radians(-2.3)] successfully completed - one_line_iron_wheel_pallet") is_in_pose() if(EMO(0,0)): continue get_distance_to_base(iron_wheel_pallet_max_z_pos) # @@@@@@@@@@@@@@@@@@@current_layer_height 에 z값 저장 (z값은 is_in_pose 함수에서 가져옴, )@@@@@@@@@@@@@@@@@@@@@ current_layer_height = z_pos # ── [적재 릴리즈] 수직 그리퍼 해제 → 푸셔 고정 → 포크 인출 → 푸셔 해제 ── robot.set_digital_output(IO_CABINET, 0, 0) # [적재] 수직 그리퍼 OFF (책을 적재면에 내려놓음) write_log("BE: set_digital_output(IO_CABINET, 0, 0) (gripper vertical cylinder off after put on pallet) successfully completed - one_line_iron_wheel_pallet") time.sleep(RELEASE_WAIT) # (기존 0.3/0.2) 수직 그리퍼 후퇴 대기 if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # [적재] 수평 푸셔 전진: 포크 인출 시 책이 딸려 나오지 않게 고정 write_log("BE: set_digital_output(IO_CABINET, 1, 1) (gripper parallel cylinder on after put on pallet) successfully completed - one_line_iron_wheel_pallet") time.sleep(PUSH_HOLD_WAIT) # (기존 1.0s) 푸셔 스트로크 완료 대기 if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -(iron_wheel_pallet_max_z_pos - (current_layer_height)), 0, 0, 0], 1, False, 8000, 3000, 0.1) write_log(f"BE:linear_move_extend : [0, 0, {-(iron_wheel_pallet_max_z_pos - (current_layer_height))}, 0, 0, 0] (move robot little bit up) successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 0) # [적재] 수평 푸셔 OFF → 홈 복귀 후 다음 사이클 write_log("BE: set_digital_output(IO_CABINET, 1, 0) (after lift up turn OFF gripper parallel cylinder) successfully completed - one_line_iron_wheel_pallet") if (EMO(1, 0)): continue robot.joint_move_extend(home_pos, 0, False, 8000, 3000, 0.1) write_log("BE: joint_move_extend : home_pos successfully completed - one_line_iron_wheel_pallet") is_in_pose() if (EMO(1, 0)): continue current_layer_index += 1 oneLine_data[grid_id]['currentLayerHeight'] = current_layer_height oneLine_data[grid_id]['currentLayerIndex'] = current_layer_index with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) print(f"현재{current_layer_index}층, 현재 높이 : {current_layer_height}") is_move_pose = True power_on_stat = 0 enabled_stat = 0 emergency_stop = 0 protected_stop_stat = 0 current_DI_9_status = 0 # jks_status = 0 def robot_get_status_realtime(): """로봇 상태 폴링 스레드를 시작한다 (모듈 로드 시 1회 호출). 0.1초 주기로 get_robot_status()를 읽어 전역 상태(전원/서보/비상정지/DI 센서)를 갱신하고, 경광등·컨베이어 인터록과 비상정지 해제 후 홈 복귀를 담당한다. """ def get_status_thread(): global power_on_stat global enabled_stat global emergency_stop global protected_stop_stat global current_DI_9_status global action global terminate_robot_button_clicked, robot_start_clicked global stacker_thread_running, stacker_thread_operator global flag global btn_id global is_emo_disabled, stacker_vertical_cyl_sensor, stacker_book_ready_sensor, stacker_vertical_cyl_down_sensor if os.path.exists(cache_json_file_path) and os.path.getsize(cache_json_file_path) > 0: with open(cache_json_file_path, 'r', encoding='utf-8') as file: cache_data = json.load(file) current_layer_height_cache = cache_data.get('currentLayerHeight') current_layer_height = current_layer_height_cache else: cache_data = { "currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight" : 0 } with open(cache_json_file_path, 'w', encoding='utf-8') as file: json.dump(cache_data, file, indent=4, ensure_ascii=False) current_layer_height = 0 if os.path.exists(oneLine_json_file_path) and os.path.getsize(oneLine_json_file_path) > 0: with open(oneLine_json_file_path, 'r', encoding='utf-8') as file: oneLine_data = json.load(file) grid_data = oneLine_data.get(grid_id, {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}) current_layer_height_oneLine = grid_data.get('currentLayerHeight', 0) else: oneLine_data = { "grid1": {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid2": {"currentDataIndex": 1, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid3": {"currentDataIndex": 2, "currentLayerIndex": 0, "currentLayerHeight": 0}, "grid4": {"currentDataIndex": 3, "currentLayerIndex": 0, "currentLayerHeight": 0} } with open(oneLine_json_file_path, 'w', encoding='utf-8') as file: json.dump(oneLine_data, file, indent=4, ensure_ascii=False) current_layer_height_oneLine = 0 # 경광등 마지막 전송값 캐시 (값이 바뀔 때만 DO를 쓰기 위함) last_lamp = None lamp = None lamp_refresh_count = 0 while True: try: status = robot.get_robot_status() power_on_stat = status[1][2] # 전원 ON 여부 enabled_stat = status[1][3] # 서보 enable 여부 emergency_stop = status[1][23] # 비상정지 눌림 여부 protected_stop_stat = status[1][5] # 보호정지(충돌 감지) 여부 current_DI_9_status = status[1][11][0] # DI0: 스태커 책 인입 반사판 센서 stacker_book_ready_sensor = status[1][11][1] # DI1: 스태커 스택 준비(픽업량 도달) 센서 stacker_vertical_cyl_sensor = status[1][11][2] # DI2: 스태커 리프터 '위' 센서 stacker_vertical_cyl_down_sensor = status[1][11][3] # DI3: 스태커 리프터 '아래' 센서 # 경광등 조작하기 위해서 쌓인 책 높이 받아오기 with open(cache_json_file_path, 'r', encoding='utf-8') as file: cache_data = json.load(file) current_layer_height_cache = cache_data.get('currentLayerHeight') current_layer_height = current_layer_height_cache with open(oneLine_json_file_path, 'r', encoding='utf-8') as file: oneLine_data = json.load(file) grid_data = oneLine_data.get(grid_id, {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}) current_layer_height_oneLine = grid_data.get('currentLayerHeight', 0) if stacker_vertical_cyl_down_sensor == 0 or terminate_robot_button_clicked == True: # stacker_vertical_cyl_sensor == 0 robot.set_digital_output(IO_CABINET, 5, 0) # 동력 장치 멈춤 (수직 실린더 올라가기 시작하자마자) elif stacker_vertical_cyl_sensor == 0 and (enabled_stat == 1 and (emergency_stop == 0 or protected_stop_stat == 0)) and terminate_robot_button_clicked == False: robot.set_digital_output(IO_CABINET, 5, 1) # 동력 장치 재시작 (수직 실린더 완전히 내려갔을때) # elif stacker_vertical_cyl_down_sensor == 1 and (enabled_stat == 1 and (emergency_stop == 0 or protected_stop_stat == 0)): # robot.set_digital_output(IO_CABINET, 5, 1) # 동력 장치 재시작 (수직 실린더 완전히 내려갔을때) if((terminate_robot_button_clicked == True) or (((wheel_max_current_layer_height < current_layer_height + 130 < wheel_max_current_layer_height + 130) or (wheel_max_current_layer_height < current_layer_height_oneLine + 130 < wheel_max_current_layer_height + 130)) and (pallet_height == wooden_wheel_pallet_height or pallet_height == iron_wheel_pallet_height)) or (flat_max_current_layer_height < current_layer_height + 130 < flat_max_current_layer_height + 130)): lamp = (0, 0, 1) # 빨강: 종료/최대 적재 도달/비상정지 elif((enabled_stat == 1) and (emergency_stop == 0 or protected_stop_stat == 0) and (((wheel_max_current_layer_height - 130 < current_layer_height + 130 < wheel_max_current_layer_height) or (wheel_max_current_layer_height - 130 < current_layer_height_oneLine + 130 < wheel_max_current_layer_height)) and (pallet_height == wooden_wheel_pallet_height or pallet_height == iron_wheel_pallet_height)) or (flat_max_current_layer_height - 130 < current_layer_height + 130 < flat_max_current_layer_height)): lamp = (1, 0, 0) # 주황: 마지막 층 적재 근접 예고 elif(enabled_stat == 1 and (robot_start_clicked == True) and (emergency_stop == 0 or protected_stop_stat == 0)): lamp = (0, 1, 0) # 초록: 정상 운전 중 elif(emergency_stop == 1 or protected_stop_stat == 1): lamp = (0, 0, 1) # 빨강: 종료/최대 적재 도달/비상정지 # 경광등 쓰기: 같은 값을 0.1초마다 재전송하면 로봇 통신만 점유하므로 # 값이 바뀔 때만 쓰고, 3초(30주기)마다 한 번 재전송하여 상태를 복구한다. lamp_refresh_count += 1 if lamp is not None and (lamp != last_lamp or lamp_refresh_count >= 30): lamp_refresh_count = 0 last_lamp = lamp robot.set_digital_output(IO_CABINET, 2, lamp[0]) # 경광등 주황 robot.set_digital_output(IO_CABINET, 3, lamp[1]) # 경광등 초록 robot.set_digital_output(IO_CABINET, 4, lamp[2]) # 경광등 빨강 # ── 비상정지/보호정지 발생: 스태커·컨베이어 정지, 복구 플래그 세팅 ── if(emergency_stop == 1 or protected_stop_stat == 1): if flag == 0: write_log("BE: emergency or protected enabled") # 최초 1회만 기록 (기존: 0.1s마다 반복 기록) is_emo_disabled = False flag = 1 stacker_thread_running = False if stacker_thread_operator: stacker_thread_operator.join() robot.set_digital_output(IO_CABINET, 5, 0) write_log("BE: robot.set_digital_output(IO_CABINET, 5, 0) conveyor belt off") # ── 비상 해제 감지: 안전 높이(-350mm) 리프트 → 홈 복귀 → 스태커 재시작 ── if(flag == 1 and enabled_stat == 1 and (emergency_stop == 0 and protected_stop_stat == 0)): write_log("BE: after emergency or protected disabled") is_emo_disabled = True flag = 0 robot.linear_move_extend([0, 0, -350, 0, 0, 0], 1, True, 7000, 2000, 0.1) write_log("BE: linear_move_extend : [0, 0, -350, 0, 0, 0] successfully completed - get_status_thread") time.sleep(0.3) home_pos = [radians(-58.423), radians(95.567), radians(66.994), radians(107.440), radians(-90), radians(31.577)] robot.joint_move_extend(home_pos, 0, True, 8000, 3000, 0.1) robot.set_digital_output(IO_CABINET, 1, 0) write_log("BE: joint_move_extend : home_pos successfully completed - get_status_thread") stacker_thread_start_function() except Exception as e: # pass print(e) finally: time.sleep(0.1) status_thread = threading.Thread(target=get_status_thread, daemon=True) status_thread.start() def stacker_thread_start_function(): """스태커(하부 적층 장치) 실린더 제어 스레드를 시작한다. 동작 원리: 책이 들어오면(DI0) 리프터(coil 0)가 책을 위로 밀어올리고, 그 사이 선반(coil 1)이 열렸다 닫히면서 책이 선반 위 스택 맨 아래에 끼워진다. 스택이 픽업량(DI1=1)에 도달하면 로봇이 가져갈 때까지 대기한다. (자세한 원리는 파일 상단 [스태커 동작 원리] 참고) """ global stacker_thread_running, stacker_thread_operator def stacker_thread(): Write_All_Do([0, 0, 0, 0, 0, 0, 0, 0]) # 시퀀스 시작 전 실린더 전체 OFF global emergency_stop global protected_stop_stat global current_DI_9_status global stacker_thread_running global stacker_book_ready_sensor # 스태커에 책이 어느정도 스택되면 가려지는 센서 (이 센서가 활성화 되면 스태커 동작을 멈추어야 함) global stacker_vertical_cyl_down_sensor, instrument robot.set_digital_output(IO_CABINET, 5, 1) # 동력장치 시작 while stacker_thread_running: try: instrument.write_bit(1, True) # 선반 닫힘 유지 (스택 받침 기본 상태) if stacker_book_ready_sensor == 1: # 스택 픽업량 도달 → 로봇이 가져갈 때까지 스태커는 쉰다. # (기존 2.0s → 0.5s: 픽업 직후 스태커 재개가 최대 1.5s 빨라짐) time.sleep(STACKER_BACKOFF) elif current_DI_9_status == 1 and stacker_book_ready_sensor == 0 and (emergency_stop == 0 or protected_stop_stat == 0): # 책 인입 감지 → 하부 적층 1사이클: # 리프터 상승(책+스택을 선반 위로) → 선반 열기 → 선반 닫기 → 리프터 하강 print(f"스태커 스레드 current_DI_9_status: {current_DI_9_status}") time.sleep(STACKER_LIFT_START_WAIT) instrument.write_bit(0, True) # 리프터 상승 (책을 위로 밀어올림) time.sleep(STACKER_SHELF_OPEN_DELAY) instrument.write_bit(1, False) # 선반 열기 (책이 선반 높이를 통과하도록) time.sleep(STACKER_SHELF_OPEN_TIME) instrument.write_bit(1, True) # 선반 닫기 (스택을 다시 받침) time.sleep(STACKER_SHELF_CLOSE_SETTLE) instrument.write_bit(0, False) # 리프터 하강 (스택은 선반 위에 걸쳐 남음) if emergency_stop == 1 or protected_stop_stat == 1: instrument.write_bit(1, False) # 비상 시 선반 열림 (실린더 압력 해제) except Exception: pass # Modbus 일시 오류는 무시하고 다음 주기에 재시도 finally: time.sleep(0.1) # 스태커 폴링 주기 stacker_thread_running = True stacker_thread_operator = threading.Thread(target=stacker_thread, daemon=True) stacker_thread_operator.start() def is_in_pose(): """직전에 보낸 non-block 모션 명령이 목표 위치에 도달할 때까지 대기한다. - 명령 직후에는 컨트롤러가 '이동 시작'을 아직 반영하지 못해 이전 위치 기준으로 in_pos=1 을 돌려줄 수 있으므로, IN_POS_GUARD_TIME 만큼 기다린 뒤 폴링을 시작한다. - 통신 오류(err != 0) 시에는 '미도달'로 간주하고 계속 재시도한다. (도달 확인 전에 다음 동작(그리퍼 작동 등)으로 넘어가지 않기 위한 안전장치) """ time.sleep(IN_POS_GUARD_TIME) # (기존 0.1s) in-pos 오판 방지 가드 in_pose = 0 while not in_pose: err, in_pose = robot.is_in_pos() if err != 0: in_pose = 0 # 통신 오류 → 미도달로 간주하고 재시도 if not in_pose: time.sleep(IN_POS_POLL_INTERVAL) # busy-wait 방지 (기존: 간격 없이 연속 폴링) z_pos = 0 # 높이 탐색 결과(현재 적재면의 z값). 메인 루프가 current_layer_height 로 사용 def get_distance_to_base(max_z_pos): """적재 지점 상공에서 적재면 근처까지 하강한 뒤, 접촉 탐색으로 실제 적재 높이를 측정한다. max_z_pos: 해당 팔레트 바닥까지의 최대 하강 거리(mm). 동작 순서: 1) 진행 상태 json에서 현재 층수/누적 높이를 읽음 2) 2층 이상이면 '실측 누적 높이', 미만이면 '계산 높이(층수 x 130mm)' 기준으로 적재면 약 130mm 위까지 고속 하강 3) 컨트롤러 저장 JKS 프로그램 'pallet_test'로 접촉 탐색 후 실측 z값을 전역 z_pos 에 저장 (책 눌림량이 매번 달라 실측값을 사용) """ global btn_id global z_pos if btn_id == "normalButton": if os.path.exists(cache_json_file_path) and os.path.getsize(cache_json_file_path) > 0: with open(cache_json_file_path, 'r', encoding='utf-8') as file: cache_data = json.load(file) current_layer_index_cache = cache_data.get('currentLayerIndex') current_layer_height_cache = cache_data.get('currentLayerHeight') current_layer_index = current_layer_index_cache current_layer_height = current_layer_height_cache if current_layer_index >= 2: robot.linear_move_extend([0, 0, max_z_pos - (current_layer_height + 130), 0, 0, 0], 1, False, speed, acc, 0.1) write_log(f"BE: normalButton / linear_move_extend : [0, 0, {max_z_pos - (current_layer_height + 130)}, 0, 0, 0] successfully completed - get_distance_to_base") is_in_pose() else: if(selectedPalletId == "woodenFlatPallet"): robot.set_payload(mass= 15, centroid =[0,0,0]) write_log("Set payload 15") print("woodenFlatPallet : currnet_layer_index 가 2보다 작아서 여기 실행") robot.linear_move_extend([0, 0, max_z_pos - (current_layer_index * 130) - 50, 0, 0, 0], 1, False, 2500, 1000, 0.1) write_log(f"BE: normalButton / linear_move_extend : [0, 0, {max_z_pos - (current_layer_index * 130)}, 0, 0, 0] successfully completed - get_distance_to_base") is_in_pose() else: print("currnet_layer_index 가 2보다 작아서 여기 실행") robot.linear_move_extend([0, 0, max_z_pos - (current_layer_index * 130), 0, 0, 0], 1, False, speed, acc, 0.1) write_log(f"BE: normalButton / linear_move_extend : [0, 0, {max_z_pos - (current_layer_index * 130)}, 0, 0, 0] successfully completed - get_distance_to_base") is_in_pose() elif btn_id == "oneLineButton": if os.path.exists(oneLine_json_file_path) and os.path.getsize(oneLine_json_file_path) > 0: with open(oneLine_json_file_path, 'r', encoding='utf-8') as file: oneLine_data = json.load(file) grid_data = oneLine_data.get(grid_id, {"currentDataIndex": 0, "currentLayerIndex": 0, "currentLayerHeight": 0}) current_layer_index_oneline = grid_data.get('currentLayerIndex', 0) current_layer_height_oneline = grid_data.get('currentLayerHeight', 0) if current_layer_index_oneline >= 2: robot.linear_move_extend([0, 0, max_z_pos - (current_layer_height_oneline + 130), 0, 0, 0], 1, False, speed, acc, 0.1) write_log(f"BE: oneLineButton / linear_move_extend : [0, 0, {max_z_pos - (current_layer_height_oneline + 130)}, 0, 0, 0] successfully completed - get_distance_to_base") is_in_pose() else: robot.linear_move_extend([0, 0, max_z_pos - (current_layer_index_oneline * 130), 0, 0, 0], 1, False, speed, acc, 0.1) write_log(f"BE: oneLineButton / linear_move_extend : [0, 0, {max_z_pos - (current_layer_index_oneline * 130)}, 0, 0, 0] successfully completed - get_distance_to_base") is_in_pose() # ── 적재면 접촉 탐색: 컨트롤러 저장 JKS 프로그램 'pallet_test' 실행 ── robot.program_load("pallet_test") print(robot.get_loaded_program()) robot.program_run() while True: default_program_status = robot.get_program_state() jks_status = default_program_status[1] # 0=종료, 1=실행중, 2=일시정지 if jks_status == 0: # 탐색이 끝난 지점의 현재 z값을 실측 적재 높이로 기록. # 통신 오류로 관절값이 오염되면 다음 층 적재 높이가 통째로 틀어지므로 # 정상 수신(err==0)될 때까지 재시도한다. while True: err_joint, joint_val = robot.get_joint_position() if err_joint == 0: break time.sleep(IN_POS_POLL_INTERVAL) cartesian_pos = robot.kine_forward(joint_val) z_pos = -(cartesian_pos[1][2]) write_log(f"BE: z_pos : {z_pos} successfully completed - get_distance_to_base") break time.sleep(JKS_POLL_INTERVAL) # busy-wait/과다 print 방지 (기존: 간격 없이 폴링하며 매회 print) def EMO(dio_number, status): """비상정지/보호정지 여부를 확인하고, 정지 중이면 그리퍼 DO를 지정 상태로 유지한다. 적재 시퀀스 곳곳에서 `if (EMO(x, y)): continue` 형태로 호출된다. - 반환 True : 정지 중 → 호출부는 현재 사이클을 중단하고 메인 루프 처음으로 복귀 - 반환 False : 정상 → 시퀀스 계속 진행 dio_number/status 는 정지 시점에 유지할 그리퍼 상태: 예) EMO(0, 1) = 책을 물고 있는 구간 → 수직 그리퍼 ON 유지 (책 낙하 방지) EMO(0, 0) = 빈 손 구간 → 수직 그리퍼 OFF 유지 """ if (emergency_stop == 1 or protected_stop_stat == 1) and (power_on_stat == 0 and enabled_stat == 0): robot.set_digital_output(IO_CABINET, dio_number, status) robot.set_digital_output(IO_CABINET, 3, 0) # 경광등 초록 OFF time.sleep(0.1) # 정지 중 메인 루프의 고속 반복 호출로 컨트롤러 통신이 몰리는 것 방지 return True return False # 모듈 로드 시점부터 상태 폴링 스레드 가동 (앱 실행 즉시 센서/상태 감시 시작) robot_get_status_realtime()