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" 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' # robot_ip_address = '192.168.56.102' robot = jkrc.RC(robot_ip_address) IO_CABINET = 0 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') wooden_wheel_pallet_height = 653 iron_wheel_pallet_height = 633 pallet_width = 0 pallet_length = 0 pallet_height = 0 book_center_positions = [] grid_id = 0 wheel_max_current_layer_height = 720 # 현재 레이어 최대 적재 높이 - 120 mm flat_max_current_layer_height = 1080 # 현재 레이어 최대 적재 높이 - 120 mm oneline_wheel_max_current_layer_height = 720 robot_start_clicked = False terminate_robot_button_clicked = False stacker_thread_running = False stacker_thread_operator = None action = None immediately_take_book = None is_one_line_stop_clicked = None btn_id = None flag = 0 # 비상정지버튼 눌렸는지 안눌렸는지에 대한 플래그 speed = 7000 acc = 2300 grid_speed = 10 grid_acc = 1 is_in_pose_time = 0.1 is_emo_disabled = True save_time = 2 stacker_vertical_cyl_sensor = 0 stacker_vertical_cyl_down_sensor = 0 stacker_book_ready_sensor = 0 wooden_wheel_pallet_max_z_pos = 864 iron_wheel_pallet_max_z_pos = 884 wooden_Flat_pallet_max_z_pos = 1192 # 227 port = 'COM3' baudrate = 115200 timeout = 2 # 모드버스 디바이스 설정 slave_id = 1 # Slave ID는 1 # Function code: 0x02 (Read Discrete Inputs) start_address = 0x0000 # 시작 주소 num_points = 8 # 읽을 포인트 수 (8개) num_coils = 8 # 쓰기 할 코일의 개수 instrument = minimalmodbus.Instrument(port, slave_id) # 포트와 슬레이브 ID 지정 instrument.serial.baudrate = baudrate # 보드레이트 설정 instrument.serial.timeout = timeout # 타임아웃 설정 instrument.mode = minimalmodbus.MODE_RTU # RTU 모드 설정 instrument.close_port_after_each_call = True # 포트를 자동으로 닫음 def Write_All_Do(coils_values): global instrument coils_values = coils_values # 최소한의 모드버스 연결 객체 생성 try: # Function Code 15 - 여러 코일 값 쓰기 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]) def write_log(message): 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): 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}") @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(): 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 @api_routes.route('/robot_start', methods=['POST']) def robot_start(): 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(): 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(): 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(): 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(): 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(): 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(): 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(): 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(): 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(): 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): 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(): 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: if (EMO(0, 0)): is_move_pose = True continue 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 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) 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) # 공중에서 z축 내리기(-247.633) 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) # (-273.633) -253.633 --> -249.633 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 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: # 홀수 번째 레이어 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'] 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(save_time) robot.linear_move_extend([-298.306, 0, 0,0,0,0], 1, False, 8000, 3000, 0.1) # x축 -해서 책 집을 위치로 가기 (-67.025) 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) # 그리퍼 수직 작동 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(0.3) 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(0.3) 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(0.3) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -180, 0, 0, 0], 1, False, 8000, 3000, 0.1) 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(0.5) 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: # 짝수 번째 레이어 (그리퍼 돌려서 책 들기) 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'] 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(save_time) robot.linear_move_extend([194.344,0,0,0,0,0], 1, False, 8000, 3000, 0.1) # (-386.213) --> -387.213으로 하기 -->-384.208 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(0.1) if (EMO(0, 0)): continue robot.linear_move_extend([0,0,-703.816,0,0,0], 1, False, speed, acc, 0.1) # (-927.623) 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(0.3) if centerY > pallet_length /2: robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 5, 1, 0.1) # 만약 책이 미끄러진다면 속도 조절 둘다 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(0.2) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -180, 0,0, 0], 1, False, 8000, 3000, 0.1) 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(0.5) 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(): 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: if (EMO(0, 0)): is_move_pose = True continue 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 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) 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) # 공중에서 z축 내리기(-247.633) 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) # (-273.633) -253.633 --> -249.633 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 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: # 홀수 번째 레이어 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'] 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(save_time) robot.linear_move_extend([-298.306, 0, 0,0,0,0], 1, False, 8000, 3000, 0.1) # x축 -해서 책 집을 위치로 가기 (-67.025) 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) # 그리퍼 수직 작동 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(0.3) 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(0.3) 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(0.3) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) if (EMO(1, 1)): continue robot.linear_move_extend([0,0, -180, 0,0, 0], 1, False, 8000, 3000, 0.1) 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(0.5) 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: # 짝수 번째 레이어 (그리퍼 돌려서 책 들기) 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'] 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(save_time) robot.linear_move_extend([194.344,0,0,0,0,0], 1, False, 8000, 3000, 0.1) # (-386.213) --> -387.213으로 하기 -->-384.208 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(0.1) if (EMO(0, 0)): continue robot.linear_move_extend([0,0,-703.816,0,0,0], 1, False, speed, acc, 0.1) # (-927.623) 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(0.3) if centerY > pallet_length /2: robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 5, 1, 0.1) # 만약 책이 미끄러진다면 속도 조절 둘다 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(0.2) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) if (EMO(1, 1)): continue robot.linear_move_extend([0,0, -180, 0,0, 0], 1, False, 8000, 3000, 0.1) 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(0.5) 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(): 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: if (EMO(0, 0)): is_move_pose = True continue 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 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) 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) # 공중에서 z축 내리기(-247.633) 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) # (-273.633) -253.633 --> -249.633 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 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: # 홀수 번째 레이어 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'] 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(save_time) robot.linear_move_extend([-298.306, 0, 0,0,0,0], 1, False, 8000, 3000, 0.1) # x축 -해서 책 집을 위치로 가기 (-67.025) 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) # 그리퍼 수직 작동 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(0.3) 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(0.3) 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(0.3) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) if (EMO(1, 1)): continue robot.linear_move_extend([0,0, -180, 0,0, 0], 1, False, 8000, 3000, 0.1) 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(0.5) 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: # 짝수 번째 레이어 (그리퍼 돌려서 책 들기) 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'] 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(save_time) robot.linear_move_extend([194.344,0,0,0,0,0], 1, False, 8000, 3000, 0.1) # (-386.213) --> -387.213으로 하기 -->-384.208 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(0.1) if (EMO(0, 0)): continue robot.linear_move_extend([0,0,-703.816,0,0,0], 1, False, speed, acc, 0.1) # (-927.623) 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(0.3) if centerY < pallet_length /2: 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 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(0.3) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) if (EMO(1, 1)): continue robot.linear_move_extend([0, 0, -180, 0, 0, 0], 1, False, 8000, 3000, 0.1) 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(0.5) 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 # currentDataIndex는 고정값인 것 잊지 않기 def one_line_wooden_wheel_pallet(): 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: if (EMO(0, 0)): is_move_pose = True continue # 로봇 동작 종료 누르면 초기화 및 while 탈출 / 스태커 멈춤 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 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) # 공중에서 z축 내리기(-247.633) 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) # (-273.633) -253.633 --> -249.633 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 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: # 홀수 번째 레이어 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'] 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(save_time) robot.linear_move_extend([-298.306, 0, 0,0,0,0], 1, False, 8000, 3000, 0.1) # x축 -해서 책 집을 위치로 가기 (-67.025) 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) # 그리퍼 수직 작동 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(0.3) 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(0.3) 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(0.3) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) 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: # 짝수 번째 레이어 (그리퍼 돌려서 책 들기) 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'] 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(save_time) robot.linear_move_extend([194.344,0,0,0,0,0], 1, False, 8000, 3000, 0.1) # (-386.213) --> -387.213으로 하기 -->-384.208 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(0.1) if (EMO(0, 0)): continue robot.linear_move_extend([0,0,-703.816,0,0,0], 1, False, speed, acc, 0.1) # (-927.623) 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(0.3) if centerY > pallet_length /2: robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 5, 1, 0.1) # 만약 책이 미끄러진다면 속도 조절 둘다 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(0.2) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) 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(): 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: if (EMO(0, 0)): is_move_pose = True continue 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 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) # 공중에서 z축 내리기(-247.633) 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) # (-273.633) -253.633 --> -249.633 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 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: # 홀수 번째 레이어 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'] 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(save_time) robot.linear_move_extend([-298.306, 0, 0,0,0,0], 1, False, 8000, 3000, 0.1) # x축 -해서 책 집을 위치로 가기 (-67.025) 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) # 그리퍼 수직 작동 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(0.3) 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(0.3) 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(0.3) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) 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: # 짝수 번째 레이어 (그리퍼 돌려서 책 들기) 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'] 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(save_time) robot.linear_move_extend([194.344,0,0,0,0,0], 1, False, 8000, 3000, 0.1) # (-386.213) --> -387.213으로 하기 -->-384.208 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(0.1) if (EMO(0, 0)): continue robot.linear_move_extend([0,0,-703.816,0,0,0], 1, False, speed, acc, 0.1) # (-927.623) 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(0.3) if centerY > pallet_length /2: robot.joint_move_extend(lower_pallet_enter_pose, 0, False, 5, 1, 0.1) # 만약 책이 미끄러진다면 속도 조절 둘다 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(0.2) if (EMO(0, 0)): continue robot.set_digital_output(IO_CABINET, 1, 1) # 그리퍼 수평 on 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(1) 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(): 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 while True: try: status = robot.get_robot_status() power_on_stat = status[1][2] enabled_stat = status[1][3] emergency_stop = status[1][23] protected_stop_stat = status[1][5] current_DI_9_status = status[1][11][0] # 스태커 수직 반사판 센서 stacker_book_ready_sensor = status[1][11][1] # 스태커 수평 반사판 샌서 stacker_vertical_cyl_sensor = status[1][11][2] # 스태커 수직 실린더 위에 센서 stacker_vertical_cyl_down_sensor = status[1][11][3] # 스태커 수직 실린더 아래 센서 # 경광등 조작하기 위해서 쌓인 책 높이 받아오기 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)): robot.set_digital_output(IO_CABINET, 2, 0) # 경광등 주황색 off robot.set_digital_output(IO_CABINET, 3, 0) # 경광등 초록색 off robot.set_digital_output(IO_CABINET, 4, 1) # 경광등 빨간색 on 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)): robot.set_digital_output(IO_CABINET, 2, 1) # 경광등 주황색 on robot.set_digital_output(IO_CABINET, 3, 0) # 경광등 초록색 off robot.set_digital_output(IO_CABINET, 4, 0) # 경광등 빨간색 off elif(enabled_stat == 1 and (robot_start_clicked == True) and (emergency_stop == 0 or protected_stop_stat == 0)): robot.set_digital_output(IO_CABINET, 2, 0) # 경광등 주황색 off robot.set_digital_output(IO_CABINET, 3, 1) # 경광등 초록색 on robot.set_digital_output(IO_CABINET, 4, 0) # 경광등 빨간색 off elif(emergency_stop == 1 or protected_stop_stat == 1): robot.set_digital_output(IO_CABINET, 2, 0) # 경광등 주황색 off robot.set_digital_output(IO_CABINET, 3, 0) # 경광등 초록색 off robot.set_digital_output(IO_CABINET, 4, 1) # 경광등 빨간색 on if(emergency_stop == 1 or protected_stop_stat == 1): write_log("BE: emergency or protected enabled") 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") 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(): global stacker_thread_running, stacker_thread_operator def stacker_thread(): Write_All_Do([0,0,0,0,0,0,0,0]) 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: time.sleep(2) # 이 센서가 활성화 되면 로봇이 가져가야하기 때문에 time을 주어야 할 것 같음. elif current_DI_9_status == 1 and stacker_book_ready_sensor == 0 and (emergency_stop == 0 or protected_stop_stat == 0): print(f"스태커 스레드 current_DI_9_status: {current_DI_9_status}") time.sleep(0.5) instrument.write_bit(0, True) time.sleep(0.05) # robot.set_digital_output(IO_CABINET, 5, 0) # 동력 off instrument.write_bit(1, False) time.sleep(1.2) instrument.write_bit(1, True) time.sleep(0.2) instrument.write_bit(0, False) # robot.set_digital_output(IO_CABINET, 5, 1) # 동력 ON if emergency_stop == 1 or protected_stop_stat == 1: instrument.write_bit(1, False) except Exception as e: pass 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(): time.sleep(0.1) in_pose = 0 while not in_pose: err, in_pose = robot.is_in_pos() z_pos = 0 def get_distance_to_base(max_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() robot.program_load("pallet_test") robot.get_loaded_program() print(robot.get_loaded_program()) robot.program_run() while True: default_program_status = robot.get_program_state() jks_status = default_program_status[1] print(f"jks_statusL {jks_status}") if jks_status == 0: print("jsk 실행 여부") print(jks_status) joint_pos = robot.get_joint_position() cartesian_pos = robot.kine_forward(joint_pos[1]) z_pos = -(cartesian_pos[1][2]) write_log(f"BE: z_pos : {z_pos} successfully completed - get_distance_to_base") break def EMO(dio_number, status): 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 return True return False robot_get_status_realtime()