update 完成复位等逻辑

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)