diff --git a/COM/COM_Robot.py b/COM/COM_Robot.py index e0b5a7a..172a5f2 100644 --- a/COM/COM_Robot.py +++ b/COM/COM_Robot.py @@ -16,7 +16,7 @@ class DetectType(Enum): class RobotClient(TCPClient): - def __init__(self, ip, port, photo_locs,command_quene, status_model:DataAddress,con_ios, time_delay_take,time_delay_put,time_delay_shake): + def __init__(self, ip, port, photo_locs,command_quene, status_model:DataAddress,con_ios, time_delay_take,time_delay_put,time_delay_shake,origin_position): super().__init__(ip, port) self.command_quene = command_quene self.status_model = status_model @@ -27,6 +27,7 @@ class RobotClient(TCPClient): self.time_delay_put = time_delay_put self.time_delay_shake = time_delay_shake self.type_detection = DetectType.EyeOutHand + self.origin_position = origin_position def add_sendQuene(self,command): #后面 命令分等级，紧急命令直接执行 self.command_quene.put(command) diff --git a/CU/Feeding.py b/CU/Feeding.py index 2753957..b15252b 100644 --- a/CU/Feeding.py +++ b/CU/Feeding.py @@ -6,10 +6,12 @@ from PyQt5.QtWidgets import QMessageBox 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 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 @@ -134,7 +136,6 @@ class FeedLine: - class FeedingConfig: def __init__(self, num: int, feedLine: FeedLine, photo_locs): self.num = num @@ -162,6 +163,7 @@ class Feeding: self.cRis_photo = CRisOrFall() self.feed_Mid_Status = FeedMidStatus.FMid_Start self.is_reverse = False + self.run_reverse = False pass def init_detection_image(self): @@ -211,16 +213,17 @@ class Feeding: elif self.feedStatus == FeedStatus.FStart: log.log_message(logging.INFO, Constant.str_feed_start) - if not self.robotClient.get_origin_position().compare(real_position) and not self.is_reverse: + if not self.robotClient.origin_position.compare(real_position) and not self.is_reverse: QMessageBox.information(None, "提示", Constant.str_feed_start_error) self.feedStatus = FeedStatus.FNone return - if self.is_reverse and self.robotClient.get_origin_position().compare(real_position): + if self.is_reverse and self.robotClient.origin_position.compare(real_position): self.feedStatus = FeedStatus.FNone self.is_reverse = False return - # TODO 获取目标位置 + self.feedConfig.feedLine.set_take_position(real_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 @@ -366,9 +369,53 @@ class Feeding: log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}') self.next_position() + def return_original_position(self): + self.run_reverse = True + 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.feedConfig == None: return + start_index = -1 + # for index in range(len(self.feedConfig.feedLine.positions)): + # if self.feedConfig.feedLine.positions[index].status == 2: + # start_index = index + pos_index = -1 + pos_near_index = -1 + reversed_positions = [] + for index, pos_model in enumerate(self.feedConfig.feedLine.positions): + if pos_model.get_position().compare(real_position): + pos_index = index + break - - + if 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.positions): + if get_distance(pos_model.get_position(), real_position) - 5 + 2 diff --git a/Model/FeedModel.py b/Model/FeedModel.py index 375ef59..31b5ff3 100644 --- a/Model/FeedModel.py +++ b/Model/FeedModel.py @@ -53,6 +53,12 @@ class PositionModel: self.lineType = 0 self.section = f'Position{index}' pass + def init_position(self,real_pos:Real_Position): + self.set_position(real_pos) + self.id = 0 + self.order = 0 + self.status = 3 + def read_position_model(self,config_reader,index:int): self.section = f'Position{index}' diff --git a/Seting.ini b/Seting.ini index 0c3e6d6..6e04393 100644 --- a/Seting.ini +++ b/Seting.ini @@ -55,6 +55,14 @@ takedelay = 0.2 putdelay = 0.1 shakedelay = 1.0 +[Origin] +X = 0.0 +Y = 0.0 +Z = 0.0 +U = 0.0 +V = 0.0 +W = 0.0 + [Camera_Feed] ipaddress = 127.0.0.1 diff --git a/Util/util_math.py b/Util/util_math.py new file mode 100644 index 0000000..c0974f2 --- /dev/null +++ b/Util/util_math.py @@ -0,0 +1,11 @@ +import math + + +def get_distance(p1, p2): + """ + 计算两点间的距离 + :param p1: + :param p2: + :return: + """ + return math.sqrt((p1.X - p2.X) ** 2 + (p1.Y - p2.Y) ** 2+ (p1.Z - p2.Z)**2) \ No newline at end of file diff --git a/main.py b/main.py index dc871e0..82fa52a 100644 --- a/main.py +++ b/main.py @@ -3,6 +3,7 @@ import json import logging import queue import sys +import threading from multiprocessing import Process import traceback @@ -38,6 +39,7 @@ 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 class MainWindow(QMainWindow, Ui_MainWindow): @@ -521,6 +523,7 @@ class MainWindow(QMainWindow, Ui_MainWindow): self.lineEdit_num.setValidator(int_validator) # self.horizontalSlider_J1.sliderReleased + self.pushButton_startFeed.clicked.connect(self.send_startFeed_button_click) @@ -564,6 +567,13 @@ class MainWindow(QMainWindow, Ui_MainWindow): float(self.configReader.get('Robot_Feed', 'photo_u5')),float(self.configReader.get('Robot_Feed', 'photo_v5')),float(self.configReader.get('Robot_Feed', 'photo_w5')) ) ] + origin_position = Real_Position(float(self.configReader.get('Origin', 'X')), + float(self.configReader.get('Origin', 'Y')), + float(self.configReader.get('Origin', 'Z')), + float(self.configReader.get('Origin', 'U')), + float(self.configReader.get('Origin', 'V')), + float(self.configReader.get('Origin', 'W'))) + solenoid_valve1_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve1_addr')) solenoid_valve2_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve2_addr')) solenoid_valve3_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve3_addr')) @@ -572,7 +582,7 @@ class MainWindow(QMainWindow, Ui_MainWindow): time_delay_shake = float(self.configReader.get('Robot_Feed', 'shakeDelay')) #TODO #dropDelay_time = int(self.configReader.get('Robot_Feed', 'dropDelay_time')) - self.robotClient = RobotClient(ip, port, photo_locs, self.command_position_quene, self.status_address,[solenoid_valve1_addr, solenoid_valve2_addr, solenoid_valve3_addr],time_delay_take,time_delay_put,time_delay_shake) + self.robotClient = RobotClient(ip, port, photo_locs, self.command_position_quene, self.status_address,[solenoid_valve1_addr, solenoid_valve2_addr, solenoid_valve3_addr],time_delay_take,time_delay_put,time_delay_shake,origin_position) self.feeding = Feeding(self.robotClient, self.detection) # 临时 self.last_time = time.time() self.remain_lineName = self.configReader.get('Robot_Feed', 'remain_lineName') @@ -1109,7 +1119,28 @@ class MainWindow(QMainWindow, Ui_MainWindow): def send_reset_button_click(self): 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) + # 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) + if self.remain_lineName != '': + line_head = f'{Constant.feedLine_set_section}{self.remain_lineName}' + if self.feedLine_dict.keys().__contains__(line_head): + self.feeding.feedConfig= FeedingConfig(0, FeedLine(self.feedLine_dict[line_head].id, self.feedLine_dict[line_head].name, + self.feedLine_dict[line_head].positions), + self.feeding.robotClient.photo_locs[:]) + else: + log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error) + return + 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) + + def stop_reset_thread(self): + self.feeding.run_reverse = False + self.send_clear_auto_command() + self.send_emergency_alarm_command() def send_tabelFeedSet_itemChanged(self, item): row = item.row() @@ -1190,10 +1221,14 @@ class MainWindow(QMainWindow, Ui_MainWindow): command = self.command_quene.get() if isinstance(command, FeedCommand) and command.status == Status.Prepareing: if self.feeding.feedStatus == FeedStatus.FNone: + position_origin = PositionModel() + position_origin.init_position(self.robotClient.origin_position) + command.feed_config.feedLine.positions.insert(0,position_origin) self.feeding.feedConfig = command.feed_config self.feeding.feedStatus = FeedStatus.FStart # self.feeding.feed_Mid_Status = FeedMidStatus.FMid_Start command.status = Status.Runing + self.record_remain_num() try: self.feeding.run() diff --git a/test_ui.py b/test_ui.py index 463b910..015e209 100644 --- a/test_ui.py +++ b/test_ui.py @@ -1,10 +1,3 @@ -from enum import Enum - - - -def keepA(a,b=1,c=2,d=3): - print(a) - print('Straight') - -keepA(2) - +my_list = [1, 2, 3, 4, 5] +list_slice = list(reversed(my_list[:2])) +print(list_slice) # 输出: [5, 4, 3, 2, 1] diff --git a/view/ResetView.py b/view/ResetView.py new file mode 100644 index 0000000..c70724a --- /dev/null +++ b/view/ResetView.py @@ -0,0 +1,16 @@ +from PyQt5.QtWidgets import QDialog +from PySide6.QtCore import Signal, Slot + +from view.ui_Dialog_Reset import Ui_Dialog_reset + + +class StopDialog(QDialog, Ui_Dialog_reset): + stop_thread_signal = Signal() # 通知主线程停止信号 + def __init__(self): + super(Ui_Dialog_reset, self).__init__() + self.setupUi(self) + self.pushButton_stopFeed.clicked.connect(self.emit_stop_signal) + @Slot() + def emit_stop_signal(self): + self.stop_thread_signal.emit() + self.close() \ No newline at end of file