update 完成复位等逻辑

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