update 完成复位等逻辑

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)
 

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)<min_distance:
+                    min_distance = get_distance(pos_model.get_position(), real_position)
+                    pos_near_index = index
+            if pos_near_index != -1:
+                reversed_positions = self.feedConfig.feedLine.positions[::pos_near_index+1]
+            else:
+                return False
+        else:
+            reversed_positions = self.feedConfig.feedLine.positions[::pos_index]
+        reversed_positions = list(reversed(reversed_positions))
+        self.reverse_index = 0
+        while self.run_reverse and reversed_positions!=len(reversed_positions):
+            pos_model = reversed_positions[self.reverse_index]
+            if pos_model.lineType == LineType.CureMid.value:
+                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())
+            else:
+                self.sendTargPosition(real_position=pos_model.get_position())
+            self.reverse_index = self.reverse_index + 1
+        return True
     def send_emergency_sound(self):
         self.sendIOControl(Constant.IO_EmergencyPoint, 1)
 

Constant.py

@@ -8,6 +8,7 @@ manual_adjust_accuracy = 1
 speed = 10
 shake_speed = 20
 debug_speed=10
+return_speed = 10
 feedLine_set_section = 'FeedLine'
 position_set_section = 'Position'
 feedLine_set_file = f'.{os.sep}Config{os.sep}feedLine.ini'
@@ -57,6 +58,8 @@ str_feed_io_control = '发送IO控制: '
 str_feed_safe_error_msgbox = '未在安全位置，请先复位！'
 str_feed_shake = '摇摆'
 str_feed_start_error = '请先复位机械臂'
+str_feed_return_original_position_fail = '机械臂未按照实际路线点移动，未能寻找到适配路径点位'
+str_feed_reset_no_line_error = '未定义的线段'
 str_sys_start = '进入系统'
 str_sys_exit = '退出系统'
 str_sys_switch_tool = '切换到工具坐标'

MainWin.ui

@@ -907,7 +907,7 @@ background-color: rgb(13, 17, 40);</string>
              </sizepolicy>
             </property>
             <property name="currentIndex">
-             <number>5</number>
+             <number>2</number>
             </property>
             <widget class="QWidget" name="page_3">
              <layout class="QVBoxLayout" name="verticalLayout_7" stretch="1">

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}'

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
 

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)

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()

test_ui.py

@@ -1,10 +1,3 @@
-from enum import Enum
+my_list = [1, 2, 3, 4, 5]
-
+list_slice = list(reversed(my_list[:2]))
-
+print(list_slice)  # 输出: [5, 4, 3, 2, 1]
-
-def keepA(a,b=1,c=2,d=3):
-    print(a)
-    print('Straight')
-
-keepA(2)
-

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()