diff --git a/CU/Feeding.py b/CU/Feeding.py index ff938ab..b56494d 100644 --- a/CU/Feeding.py +++ b/CU/Feeding.py @@ -4,20 +4,27 @@ import time import cv2 from PyQt5.QtWidgets import QMessageBox from jedi.debug import speed +from tifffile.tifffile import read_lsm_positions import Constant import Expection +from Model.FeedModel import PositionModel from Model.Position import Real_Position, Detection_Position from enum import Enum, IntEnum from COM.COM_Robot import RobotClient, DetectType from Model.RobotModel import CMDInstructRequest, MoveType +from Trace.handeye_calibration import getPosition from Util.util_math import get_distance from Util.util_time import CRisOrFall from Vision.camera_coordinate_dete import Detection from Util.util_log import log from Model.RobotModel import Instruction - +class ResetStatus(Enum): + RNone = 0 + RStart = 1 + RRunging = 2 + ROk =3 class FeedStatus(IntEnum): @@ -163,8 +170,15 @@ class Feeding: self.cRis_photo = CRisOrFall() self.feed_Mid_Status = FeedMidStatus.FMid_Start self.is_reverse = False + # 复位集合 self.run_reverse = False self.take_no_photo = False + self.reset_status = ResetStatus.RNone + self.reversed_positions = [] + self.current_position = None + + self.pos_index = -1 + self.pos_near_index = -1 pass def init_detection_image(self): @@ -220,7 +234,7 @@ class Feeding: elif self.feedStatus == FeedStatus.FStart: log.log_message(logging.INFO, Constant.str_feed_start) if not self.robotClient.origin_position.compare(real_position) and not self.is_reverse: - QMessageBox.information(None, "提示", Constant.str_feed_start_error) + # QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常 self.feedStatus = FeedStatus.FNone return if self.is_reverse: @@ -230,10 +244,13 @@ class Feeding: self.feedConfig.feedLine.get_position_list() - self.feedConfig.feedLine.set_take_position(real_position) + # = self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30, + # # save_img_point=0, Height_reduce=30, width_reduce=30) + + + self.feedConfig.feedLine.set_take_position(self.get_take_position()) # TODO 获取目标位置 - # self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30, - # save_img_point=0, Height_reduce=30, width_reduce=30) + # self.feed_Mid_Status = FeedMidStatus.FMid_Start self.next_position(self.is_reverse) @@ -382,12 +399,87 @@ class Feeding: time.sleep(self.robotClient.time_delay_put) # TODO 获取目标位置 # self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30, save_img_point=0, Height_reduce=30, width_reduce=30) - self.feedConfig.feedLine.set_take_position(real_position) + self.feedConfig.feedLine.set_take_position(self.get_take_position()) self.feedConfig.num = self.feedConfig.num - 1 log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}') self.next_position() + def run_reseet(self): + real_position = Real_Position() + real_position.init_position(self.robotClient.status_model.world_0, + self.robotClient.status_model.world_1, + self.robotClient.status_model.world_2, + self.robotClient.status_model.world_3, + self.robotClient.status_model.world_4, + self.robotClient.status_model.world_5) + if self.reset_status == ResetStatus.RNone: + return + + if self.reset_status == ResetStatus.RStart: + + if self.feedConfig == None: return + # for index in range(len(self.feedConfig.feedLine.positions)): + # if self.feedConfig.feedLine.positions[index].status == 2: + # start_index = index + self.pos_index = -1 + self.pos_near_index = -1 + self.reversed_positions = [] + for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions): + if pos_model.get_position().compare(real_position): + self.pos_index = index + break + + if self.pos_index == -1: + log.log_message(logging.ERROR, Constant.str_feed_return_original_position_fail) + min_distance = 99999999 + for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions): + if get_distance(pos_model.get_position(), real_position) < min_distance: + min_distance = get_distance(pos_model.get_position(), real_position) + self.pos_near_index = index + if self.pos_near_index != -1: + self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_near_index + 1] + else: + return False + else: + self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index] + self.reversed_positions = list(reversed(self.reversed_positions)) + self.reverse_index = 0 + self.send_emergency_sound() + self.current_position = PositionModel() + self.current_position.init_position(real_position) + self.reset_status = ResetStatus.RRunging + + if self.reset_status == ResetStatus.RRunging: + if not real_position.compare(self.current_position.get_position()): + return + pos_model = self.reversed_positions[self.reverse_index] + if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点 + print('跳过抓袋点位') + pos_model = self.reversed_positions[self.reverse_index + 1] + self.reverse_index = self.reverse_index+1 + + if pos_model.lineType == LineType.CureMid.value: + pos_model1 = self.reversed_positions[self.reverse_index + 1] + self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure, + real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed) + self.current_position = pos_model1 + self.reverse_index = self.reverse_index + 2 + else: + self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed) + self.current_position = pos_model + self.reverse_index = self.reverse_index + 1 + + if self.reverse_index == len(self.reversed_positions): + self.reset_status = ResetStatus.ROk + if self.reset_status == ResetStatus.ROk: + self.reset_status = ResetStatus.RNone + self.send_emergency_stop() + + + + def return_original_position(self): + self.run_reverse = True real_position = Real_Position() real_position.init_position(self.robotClient.status_model.world_0, @@ -425,8 +517,10 @@ class Feeding: reversed_positions = list(reversed(reversed_positions)) self.reverse_index = 0 self.send_emergency_sound() + current_position = PositionModel() + current_position.init_position(real_position) while self.run_reverse and self.reverse_index!=len(reversed_positions): - if self.reverse_index!=0 and not real_position.compare(reversed_positions[self.reverse_index-1]): + if self.reverse_index!=0 and not real_position.compare(current_position.get_position()): continue print(f'复位第{self.reverse_index}') #todo 缺少比对 pos_model = reversed_positions[self.reverse_index] @@ -439,9 +533,11 @@ class Feeding: pos_model1 = reversed_positions[self.reverse_index + 1] self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure, real_position1=pos_model1.get_position(),speed= self.robotClient.reset_speed) + current_position = pos_model1 self.reverse_index = self.reverse_index+2 else: self.sendTargPosition(real_position=pos_model.get_position(),speed=self.robotClient.reset_speed) + current_position = pos_model self.reverse_index = self.reverse_index + 1 self.send_emergency_stop() return True @@ -502,8 +598,12 @@ class Feeding: self.robotClient.add_sendQuene(request_command) pass - - + def get_take_position(self): + _, img, xyz, uvw, points = self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,save_img_point=0, Height_reduce=30, width_reduce=30) + self.detection_image = img + target_position,noraml_base = getPosition(*xyz,*uvw,None,points) + position = Real_Position().init_position(*target_position[:3],*noraml_base[:3]) + return position def next_start(self,reverse=False): print(f'原点到初始位置的第{self.feedConfig.feedLine.origin2start_pos_index}') start_pos = self.feedConfig.feedLine.get_next_start_position(reverse) diff --git a/Seting.ini b/Seting.ini index 08d6abf..ff0e6fa 100644 --- a/Seting.ini +++ b/Seting.ini @@ -1,7 +1,7 @@ [Main] [Robot_Feed] -ipaddress = 192.168.20.4 +ipaddress = 127.0.0.1 port = 502 j1_min = -150 j1_max = +150 @@ -47,7 +47,7 @@ photo_v5 = 0.0 photo_w5 = 1.0 linecount = 2 remain_linename = 1 -remain_count = 0 +remain_count = 1 solenoid_valve1_addr = 3 solenoid_valve2_addr = 2 solenoid_valve3_addr = 10 diff --git a/main.py b/main.py index 132d05c..8928ba1 100644 --- a/main.py +++ b/main.py @@ -23,7 +23,7 @@ from Model.FeedModel import LineModel, PositionModel from Util.util_ini import writeFeedLine_to_ini import Constant from CU.Command import FeedCommand -from CU.Feeding import FeedLine, FeedingConfig, Feeding, FeedStatus, FeedMidStatus +from CU.Feeding import FeedLine, FeedingConfig, Feeding, FeedStatus, FeedMidStatus, ResetStatus from Util.util_log import QTextEditLogger from Util.util_time import CRisOrFall from Vision.camera_coordinate_dete import Detection @@ -39,6 +39,9 @@ from CU.Command import Status from Util.util_log import log from Vision.detect_person import DetectionPerson from ui_MainWin import Ui_MainWindow +from view.ResetView import StopDialog + + #from view.ResetView import StopDialog @@ -542,13 +545,13 @@ class MainWindow(QMainWindow, Ui_MainWindow): def init_Run(self): self.robotClient = None self.configReader = configparser.ConfigParser() - self.detection = None # Detection() #TODO 关闭图像 + self.detection =Detection() #TODO 关闭图像 self.command_position_quene = Queue() self.status_address = DataAddress() self.feedLine_dict = {} self.command_quene = Queue() self.main_threading = None - self.detection_person = None #DetectionPerson() + self.detection_person = None # DetectionPerson() self.cton_take_no_photo = CRisOrFall() self.configReader.read(Constant.set_ini) @@ -1133,6 +1136,9 @@ class MainWindow(QMainWindow, Ui_MainWindow): self.send_start_tool_command() def send_reset_button_click(self): + # TODO 清楚痕迹 + # TODO 开启自动 + line_head = self.comboBox_lineIndex.currentData() #safe_position = self.feedLine_dict[line_head].safe_position # self.send_position_command(safe_position.X, safe_position.Y, safe_position.Z, safe_position.U, safe_position.V, safe_position.W,move_type=MoveType.WORLD) @@ -1149,16 +1155,18 @@ class MainWindow(QMainWindow, Ui_MainWindow): else: log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error) return + self.feeding.reset_status = ResetStatus.RStart + dialog_reset = StopDialog() + dialog_reset.stop_thread_signal.connect(self.stop_reset_thread) else: log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error) return - thread = threading.Thread(target=self.feeding.return_original_position()) - thread.start() - # dialog_reset = StopDialog() - # dialog_reset.stop_thread_signal.connect(self.stop_reset_thread) + # thread = threading.Thread(target=self.feeding.return_original_position()) + # thread.start() + def stop_reset_thread(self): - self.feeding.run_reverse = False + self.feeding.reset_status = ResetStatus.ROk self.send_clear_auto_command() self.send_emergency_alarm_command() @@ -1256,6 +1264,7 @@ class MainWindow(QMainWindow, Ui_MainWindow): try: self.feeding.run() + self.feeding.run_reseet() except: print(Error_Code.SYS_NONEPoint) @@ -1277,7 +1286,7 @@ class MainWindow(QMainWindow, Ui_MainWindow): # return log.log_message(logging.ERROR, '人员进入安全区') - time.sleep(3) + time.sleep(1) pass def updateUI(self): @@ -1305,12 +1314,15 @@ class MainWindow(QMainWindow, Ui_MainWindow): self.label_date.setText(datetime.now().strftime("%Y-%m-%d")) self.label_time.setText(datetime.now().strftime("%H:%M:%S")) - self.updateUI_Position() - self.updateUI_label_detection() - self.updateUI_label_status() - self.updateUI_frame_sign(self.feeding.feedStatus) - self.updateUI_IOPanel() - self.updateUI_InfoMB() + # self.updateUI_Position() + # self.updateUI_label_detection() + # self.updateUI_label_status() + # self.updateUI_frame_sign(self.feeding.feedStatus) + # self.updateUI_IOPanel() + # self.updateUI_InfoMB() + + + def updateUI_InfoMB(self): if self.cton_take_no_photo.Q(self.feeding.take_no_photo,True): self.show_infomessage_box("未识别到料袋报警，请重新放料后，点击继续") @@ -1533,7 +1545,7 @@ class MainWindow(QMainWindow, Ui_MainWindow): def send_clear_alarm_command(self, pause: bool): self.send_start_tool_command() pause_command = CMDRequest() - pause_command.cmdData.append("clearAlarmContinue") + pause_command.cmdData.append("StopButton") pause_command.cmdData.append("1") request_command = pause_command.toString() print(request_command)