update 完成控制基本逻辑

CU/Feeding.py

@@ -8,50 +8,140 @@ import Constant
 import Expection
 from Model.Position import Real_Position, Detection_Position
 from enum import Enum, IntEnum
-from COM.COM_Robot import RobotClient
+from COM.COM_Robot import RobotClient, DetectType
 from Model.RobotModel import CMDInstructRequest, MoveType
 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 FeedStatus(IntEnum):
     FNone = 0
     FStart = 1
     FCheck = 2
-    FSafeP = 3
+    FMid = 3
     FPhoto = 4
     FTake = 5
-    FSafeF = 6
-    FFeedP = 7
-    FBroken = 8
-    FCut = 9
-    FShake = 10
-    FDropBag = 11
-    FFinished = 12
+    FBroken1 = 6
+    FBroken2 =7
+    FShake = 8
+    FDropBag = 9
+    FFinished = 10
+    FReverse = 11
+    FStartReverse = 12
+
+class LineType(Enum):
+    Straight = 0
+    CureMid = 2
+    CureEnd = 3
+
+
+class FeedMidStatus(Enum):
+    FMid_Start = 1
+    FMid_Take = 2
+    FMid_Feed= 3
+
+class FeedPosition:
+    def __init__(self,status:FeedStatus,position:Real_Position):
+        self.status = status
+        self.position = position
 
 class FeedLine:
-    def __init__(self,id,name,safe_position:Real_Position,broken1_position:Real_Position,broken2_position:Real_Position,shake_position:Real_Position,drop_bag_position:Real_Position):
-
-        
-        self.safe_position = safe_position
-        self.broken1_position = broken1_position
-        self.broken2_position = broken2_position
-        self.shake_position = shake_position
-        self.drop_bag_position = drop_bag_position
-
-        self.take_position = None
+    def __init__(self, id, name, feed_positions:list):
+        self.feed_positions = feed_positions
+        self.feeding2end_pos_index = 0
+        self.origin2start_pos_index = 0
+        self.start2take_pos_index = 0
         self.name = name
-        self.id=id
+        self.id = id
+        self.get_position_list()
+
+
+    def get_current_feed_position(self):
+        pos = self.feeding_to_end[self.feeding2end_pos_index]
+        return pos
+    def get_current_take_position(self):
+        pos = self.start_to_take[self.start2take_pos_index]
+        return pos
+    def get_current_start_position(self):
+        pos = self.origin_to_start[self.origin2start_pos_index]
+        return pos
+
+    def get_next_feed_position(self,reverse:bool=False):
+        pos = self.feeding_to_end[self.feeding2end_pos_index]
+        if reverse:
+            self.feeding2end_pos_index -= 1
+            if self.feeding2end_pos_index < 0:
+                self.feeding2end_pos_index = len(self.feeding_to_end) - 1
+        else:
+            self.feeding2end_pos_index += 1
+            if self.feeding2end_pos_index >= len(self.feeding_to_end):
+                self.feeding2end_pos_index = 0
+        return pos
+
+
+    def get_next_start_position(self,reverse:bool=False):
+        pos = self.origin_to_start[self.origin2start_pos_index]
+        if reverse:
+            self.origin2start_pos_index -= 1
+            if self.origin2start_pos_index < 0:
+                self.origin2start_pos_index = len(self.origin_to_start) - 1
+        else:
+            self.origin2start_pos_index += 1
+            if self.origin2start_pos_index >= len(self.origin_to_start):
+                self.origin2start_pos_index = 0
+
+        return pos
+
+    def get_next_take_position(self,reverse:bool=False):
+        pos = self.start_to_take[self.start2take_pos_index]
+        if reverse:
+            self.start2take_pos_index -= 1
+            if self.start2take_pos_index < 0:
+                self.start2take_pos_index = len(self.start_to_take) - 1
+        else:
+            self.start2take_pos_index += 1
+            if self.start2take_pos_index >= len(self.start_to_take):
+                self.start2take_pos_index = 0
+        return pos
+
+    def get_take_position(self):
+        for i in range(len(self.feeding_to_end)):
+            if self.feeding_to_end[i].status == FeedStatus.FTake.value:
+                return self.feeding_to_end[i]
+
+    def set_take_position(self,position:Real_Position):
+        for i in range(len(self.feeding_to_end)):
+            if self.feeding_to_end[i].status == FeedStatus.FTake.value:
+                self.feeding_to_end[i].set_position(position)
+
+    def get_position_list(self):
+        index_start = -1
+        for i in range(len(self.feed_positions)):
+            if self.feed_positions[i].status == FeedStatus.FCheck.value:
+                index_start = i
+                break
+        for i in range(len(self.feed_positions)):
+            if self.feed_positions[i].status == FeedStatus.FPhoto.value:
+                index_take = i
+
+        self.origin_to_start = self.feed_positions[: index_start+1]
+        self.start_to_take = self.feed_positions[index_start:index_take+1]
+        self.feeding_to_end = self.feed_positions[index_take:]
+
+
 
 
 class FeedingConfig:
-    def __init__(self, num:int, feedLine:FeedLine,photo_locs):
+    def __init__(self, num: int, feedLine: FeedLine, photo_locs):
         self.num = num
         self.feedLine = feedLine
         self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
 
-    def deal_photo_locs(self,photo_loc):
+    def deal_photo_locs(self, photo_loc):
         position_photo = Real_Position()
         position_photo.init_position(photo_loc[0], photo_loc[1], photo_loc[2], photo_loc[3], photo_loc[4], photo_loc[5])
         return position_photo
@@ -60,8 +150,8 @@ class FeedingConfig:
         pass
 
 
-class Feeding :
-    def __init__(self,robotClient:RobotClient,detection:Detection):
+class Feeding:
+    def __init__(self, robotClient: RobotClient, detection: Detection):
         self.feedConfig = None
         self.feedStatus = FeedStatus.FNone
         self.robotClient = robotClient
@@ -70,11 +160,13 @@ class Feeding :
         self.init_detection_image()
         self.pause = False
         self.cRis_photo = CRisOrFall()
-
+        self.feed_Mid_Status = FeedMidStatus.FMid_Start
+        self.is_reverse = False
         pass
 
     def init_detection_image(self):
         self.detection_image = cv2.imread(Constant.feed_sign_path)
+
     def run(self):
         # 获取事件坐标
         real_position = Real_Position()
@@ -91,18 +183,13 @@ class Feeding :
         #                             0,
         #                             0);
 
-
         if self.feedConfig == None:
             self.feedStatus = FeedStatus.FNone
-
-        elif self.feedConfig.num == 0:
+        elif self.feedConfig.num == 0 and self.feedStatus!=FeedStatus.FReverse:
             self.feedStatus = FeedStatus.FNone
+
         if self.feedStatus == FeedStatus.FNone or self.pause:
             return
-        elif self.feedStatus == FeedStatus.FStart:
-            log.log_message(logging.INFO, Constant.str_feed_start)
-            self.feedStatus = FeedStatus.FCheck if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-            pass
         elif self.feedStatus == FeedStatus.FCheck:
             log.log_message(logging.INFO, Constant.str_feed_check)
             # 1, 检查是否是三列
@@ -110,36 +197,74 @@ class Feeding :
             # if self.safe_check_columns() and self.safe_check_person():
             #     pass
             # else:
-            if self.feedConfig.num != 0:
-                self.feedStatus = FeedStatus.FSafeP if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-                self.sendTargPosition(self.feedConfig.feedLine.safe_position)
+            # if self.feedConfig.num != 0:
+            #     self.next_target()
+            # if == 原点 继续判断
+            # else:
+            #     QMessageBox.information(None, "提示", Constant.str_feed_safe_error_msgbox)
+
+            if self.is_reverse:
+                self.feed_Mid_Status = FeedMidStatus.FMid_Start
+            else:
+                self.feed_Mid_Status = FeedMidStatus.FMid_Take
+            self.next_position(self.is_reverse)
+
+        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:
+                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):
+                self.feedStatus = FeedStatus.FNone
+                self.is_reverse = False
+                return
+                # TODO 获取目标位置
+            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.feed_Mid_Status = FeedMidStatus.FMid_Start
+            self.next_position(self.is_reverse)
+
+
+        elif self.feedStatus == FeedStatus.FMid:
+            log.log_message(logging.INFO, Constant.str_feed_mid)
+            feed_pos = self.get_current_position()
+            if feed_pos.get_position().compare(real_position):
+                self.next_position(self.is_reverse)
 
 
-        elif self.feedStatus==FeedStatus.FSafeP:
-             log.log_message(logging.INFO,Constant.str_feed_safe)
-             if self.feedConfig.feedLine.safe_position.compare(real_position):
-                self.error_photo_count=0
-                self.feedStatus = FeedStatus.FPhoto if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-                #self.sendTargPosition(self.feedConfig.feedLine.photo_position)
-             #判断是哪一个问题
 
         elif self.feedStatus == FeedStatus.FPhoto:
             log.log_message(logging.INFO, Constant.str_feed_photo)
+            if self.feedConfig.num == 0:
+                log.log_message(logging.INFO, Constant.str_feed_finish)
+                self.is_reverse = True
+                self.FeedMid_Status = FeedMidStatus.FMid_Take
+                self.next_position(self.is_reverse)
+                return
+
+            if self.robotClient.type_detection == DetectType.EyeOutHand:
+                self.feed_Mid_Status = FeedMidStatus.FMid_Feed
+                self.next_position()
+                return
             detect_pos_list = []
             if not Constant.Debug:
                 try:
                     from Util.util_time import CRisOrFall
-                    if self.cRis_photo.Q(self.error_photo_count>=5, True):
+                    if self.cRis_photo.Q(self.error_photo_count >= 5, True):
                         QMessageBox.information(None, "提示", Constant.str_feed_photo_error_msgbox)
-                        self.error_photo_count=0
-                        log.log_message(logging.INFO,Constant.str_feed_photo_confirm)
+                        self.error_photo_count = 0
+                        log.log_message(logging.INFO, Constant.str_feed_photo_confirm)
+
+                        # TODO 返回初始状态
                     for pos in self.feedConfig.photo_locs:
                         self.sendTargPosition(pos)
-                        while not pos.compare(real_position) : #可以优化 TODO
-                            if self.feedStatus == FeedStatus.FNone or not self.pause :
+                        while not pos.compare(real_position):  # 可以优化 TODO
+                            if self.feedStatus == FeedStatus.FNone or not self.pause:
                                 return
                             time.sleep(0.1)
-                        code, img, xyz, uvw, mng = self.detection.get_position() #检测结果
+                        code, img, xyz, uvw, mng = self.detection.get_position()  # 检测结果
                         self.detection_image = img
                         if xyz != None:
                             log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
@@ -147,122 +272,110 @@ class Feeding :
                             from Trace.handeye_calibration import R_matrix, getPosition
                             rotation = R_matrix(self.robotClient.status_model.world_0,
                                                 self.robotClient.status_model.world_1,
-                                                self.robotClient.status_model.world_2 - 10, #TODO 这个10 需要确定
+                                                self.robotClient.status_model.world_2,
                                                 self.robotClient.status_model.world_3,
                                                 self.robotClient.status_model.world_4,
                                                 self.robotClient.status_model.world_5)
 
                             # 黄老师给我的xyz和法向量
                             target_position, noraml_base = getPosition(*xyz, *uvw, rotation, *mng)
-                            detect_pos_list.append(Real_Position().init_position(*target_position[:3],*noraml_base))
+                            detect_pos_list.append(Real_Position().init_position(*target_position[:3], *noraml_base))
                             log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
                         else:
-                            log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail+real_position.to_string())
+                            log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail + real_position.to_string())
                     z_diff, max_z_index = (lambda pts: (
                         max(pts, key=lambda p: p.Z).Z - min(pts, key=lambda p: p.Z).Z,
                         pts.index(max(pts, key=lambda p: p.Z))
                     ))(detect_pos_list)
                     if len(self.feedConfig.photo_locs) == 5:
-                        if z_diff < Constant.bag_height and len(detect_pos_list)==3: # 第一次检测到没有高度差距开始三列拍照 TODO 保留全部的开关
+                        if z_diff < Constant.bag_height and len(
+                                detect_pos_list) == 3:  # 第一次检测到没有高度差距开始三列拍照 TODO 保留全部的开关
                             # 拍照位置从五个变为三个
-                            self.feedConfig.photo_locs = [detect_pos_list[0],detect_pos_list[2],detect_pos_list[4]]
-                            self.feedConfig.feedLine.take_position = detect_pos_list[0]
+                            self.feedConfig.photo_locs = [detect_pos_list[0], detect_pos_list[2], detect_pos_list[4]]
+                            take_position = detect_pos_list[0]
                             log.log_message(logging.INFO, Constant.str_feed_takePhoto_front_finish)
                         else:
-                            self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index]
+                            take_position = detect_pos_list[max_z_index]
                             log.log_message(logging.INFO, Constant.str_feed_takePhoto_front)
                     else:
                         if z_diff < Constant.bag_height:
-                            self.feedConfig.feedLine.take_position = detect_pos_list[0]
+                            take_position = detect_pos_list[0]
                             log.log_message(logging.INFO, Constant.str_feed_takePhoto_new_line)
                         else:
-                            self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index]
+                            take_position = detect_pos_list[max_z_index]
                             log.log_message(logging.INFO, Constant.str_feed_takePhoto_line)
-                    self.feedStatus = FeedStatus.FTake if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-                    self.sendTargPosition(self.feedConfig.feedLine.take_position)
+
+                    self.feedConfig.feedLine.set_take_position(take_position) ##TODO 检查有没有异常
+                    self.next_position()
                 except:
                     log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail)
-                    self.error_photo_count +=1
+                    self.error_photo_count += 1
             else:
-                self.feedConfig.feedLine.take_position = self.feedConfig.feedLine.safe_position
-                self.sendTargPosition(self.feedConfig.feedLine.safe_position)
-                self.feedStatus = FeedStatus.FTake if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
+                self.feedConfig.feedLine.set_take_position(real_position)  ##TODO 检查有没有异常
+                self.next_position()
             log.log_message(logging.INFO, Constant.str_feed_takePhoto_move)
 
         elif self.feedStatus == FeedStatus.FTake:
             log.log_message(logging.INFO, Constant.str_feed_take)
-            if self.feedConfig.feedLine.take_position != None and self.feedConfig.feedLine.take_position.compare(real_position):
-                # 打开吸嘴并返回
-                self.sendIOControl(self.robotClient.con_ios[0],1)
-                self.sendIOControl(self.robotClient.con_ios[1],1)
-                self.sendIOControl(self.robotClient.con_ios[2],1)
-                # TODO 检测是否通 不然报警
-                time.sleep(self.robotClient.time_delay_take)
-                self.feedStatus = FeedStatus.FSafeF if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-                log.log_message(logging.INFO, Constant.str_feed_take_success)
-                self.sendTargPosition(self.feedConfig.feedLine.safe_position)
+            if self.feedConfig.feedLine.get_take_position().get_position() != None:
+                if self.feedConfig.feedLine.get_take_position().get_position().compare(real_position):
+                    # 打开吸嘴并返回
+                    self.sendIOControl(self.robotClient.con_ios[0], 1)
+                    self.sendIOControl(self.robotClient.con_ios[1], 1)
+                    self.sendIOControl(self.robotClient.con_ios[2], 1)
 
-                pass
+                    # TODO 检测是否通 不然报警
+                    self.feedConfig.feedLine.set_take_position(None)
+                    time.sleep(self.robotClient.time_delay_take)
+                    log.log_message(logging.INFO, Constant.str_feed_take_success)
+                    self.next_position()
 
-        elif self.feedStatus == FeedStatus.FSafeF:
-            log.log_message(logging.INFO, Constant.str_feed_safe)
-            if self.feedConfig.feedLine.safe_position.compare(real_position):
-                self.feedStatus = FeedStatus.FBroken if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
 
-                self.sendTargPosition(self.feedConfig.feedLine.broken1_position)
-                pass  #吸嘴开始
-
-        # elif self.feedStatus==FeedStatus.FFeedP:
-        #     log.log_message(logging.INFO, Constant.str_feed_feed)
-        #     if self.feedConfig.feedLine.feed_position.compare(real_position):
-        #         self.feedStatus = FeedStatus.FBroken
-        #         self.sendTargPosition(self.feedConfig.feedLine.broken1_position)
-        #         pass #破袋
-
-        elif self.feedStatus==FeedStatus.FBroken:
 
+        elif self.feedStatus == FeedStatus.FBroken1:
             log.log_message(logging.INFO, Constant.str_feed_broken)
+            if self.get_current_position().get_position().compare(real_position):
+                self.next_position()
 
-            if self.feedConfig.feedLine.broken1_position.compare(real_position):
-                self.sendTargPosition(self.feedConfig.feedLine.broken2_position)
-            if self.feedConfig.feedLine.broken2_position.compare(real_position):
-                self.sendTargPosition(self.feedConfig.feedLine.shake_position)
-            if self.feedConfig.feedLine.shake_position.compare(real_position): # 延迟判断，如果最后点位延迟1s，则认为阶段完成
+
+        elif self.feedStatus == FeedStatus.FBroken2:
+            log.log_message(logging.INFO, Constant.str_feed_broken)
+            if self.get_current_position().get_position().compare(real_position):
+                self.next_position()
+
+        elif self.feedStatus == FeedStatus.FShake:
+            log.log_message(logging.INFO, Constant.str_feed_shake)
+            if self.get_current_position().get_position().compare(real_position):
                 # TODO 震动方案
                 time.sleep(self.robotClient.time_delay_shake)
-                self.feedStatus = FeedStatus.FDropBag if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-                self.sendTargPosition(self.feedConfig.feedLine.safe_position)
+                self.next_position()
 
         elif self.feedStatus == FeedStatus.FDropBag:
             log.log_message(logging.INFO, Constant.str_feed_drop)
-            if self.feedConfig.feedLine.safe_position.compare(real_position):
-                self.sendTargPosition(self.feedConfig.feedLine.drop_bag_position)
-                pass
-            if self.feedConfig.feedLine.drop_bag_position.compare(real_position):
+
+            if self.get_current_position().get_position().compare(real_position):
                 self.sendIOControl(self.robotClient.con_ios[0], 0)
                 self.sendIOControl(self.robotClient.con_ios[1], 0)
                 self.sendIOControl(self.robotClient.con_ios[2], 0)
                 # TODO 检测是否断 不然报警
                 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.num = self.feedConfig.num - 1
                 log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
-                if self.feedConfig.num == 0: # 投料次数为0，直接返回
-                    log.log_message(logging.INFO, Constant.str_feed_finish)
-                    self.feedStatus = FeedStatus.FNone
-                    self.sendTargPosition(self.feedConfig.feedLine.safe_position)
-                    self.init_detection_image()
-                else:
-                    self.feedStatus = FeedStatus.FSafeP if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-                    self.sendTargPosition(self.feedConfig.feedLine.safe_position)
+                self.next_position()
+
+
 
 
     def send_emergency_sound(self):
-       self.sendIOControl(Constant.IO_EmergencyPoint, 1)
+        self.sendIOControl(Constant.IO_EmergencyPoint, 1)
 
     def send_emergency_stop(self):
         self.sendIOControl(Constant.IO_EmergencyPoint, 0)
 
-    def sendIOControl(self,IO_bit,IO_Status:int):
+    def sendIOControl(self, IO_bit, IO_Status: int):
 
         IO_command = CMDInstructRequest()
         io_instruction = Instruction()
@@ -275,7 +388,7 @@ class Feeding :
         log.log_message(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
         pass
 
-    def sendTargPosition(self,real_position,move_type:MoveType=MoveType.WORLD ,speed = Constant.speed):
+    def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=Constant.speed,real_position1=None):
         position_instruction = Instruction()
         position_instruction.speed = speed
         position_instruction.m0 = real_position.X
@@ -284,12 +397,19 @@ class Feeding :
         position_instruction.m3 = real_position.U
         position_instruction.m4 = real_position.V
         position_instruction.m5 = real_position.W
+
         position_instruction.action = move_type.value
+        if position_instruction.action == 17:
+            position_instruction.m0_p = real_position1.X
+            position_instruction.m1_p = real_position1.Y
+            position_instruction.m2_p = real_position1.Z
+            position_instruction.m3_p = real_position1.U
+            position_instruction.m4_p = real_position1.V
+            position_instruction.m5_p = real_position1.W
         instruction_command = CMDInstructRequest()
         instruction_command.instructions.append(position_instruction)
         request_command = instruction_command.toString()
 
-
         log_str = f'移动到位置:{"姿势直线"}：' \
                   f'X:{position_instruction.m0}-' \
                   f'Y:{position_instruction.m1}-' \
@@ -297,17 +417,59 @@ class Feeding :
                   f'U:{position_instruction.m3}-' \
                   f'V:{position_instruction.m4}-' \
                   f'W:{position_instruction.m5}'
-
-        try :
+        print(log_str)
+        try:
             log.log_message(logging.INFO, log_str)
         except:
             print("error")
 
-
         self.robotClient.add_sendQuene(request_command)
         pass
 
 
+    def next_start(self,reverse=False):
+        start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
+        self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
+        if start_pos.lineType == LineType.CureMid.value:
+            start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
+            self.sendTargPosition(real_position=start_pos.get_position(), move_type=MoveType.Cure, real_position1=start_pos1.get_position())
+        else:
+            self.sendTargPosition(real_position=start_pos.get_position())
+        pass
+
+    def next_take(self,reverse=False):
+        take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
+        self.feedStatus = FeedStatus(take_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
+        if take_pos.lineType == LineType.CureMid.value:
+            take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
+            self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure, real_position1=take_pos1.get_position())
+        else:
+            self.sendTargPosition(real_position=take_pos.get_position())
+        pass
+
+    def next_Feed(self,reverse=False):
+        feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
+        self.feedStatus = FeedStatus(feed_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
+        if feed_pos.lineType == LineType.CureMid.value:
+            feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
+            self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure, real_position1=feed_pos1.get_position())
+        else:
+            self.sendTargPosition(real_position=feed_pos.get_position())
+
+    def get_current_position(self):
+        if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
+            return self.feedConfig.feedLine.get_current_start_position()
+        elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
+            return self.feedConfig.feedLine.get_current_take_position()
+        elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
+            return self.feedConfig.feedLine.get_current_feed_position()
+    def next_position(self,reverse=False):
+        if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
+            self.next_start(reverse)
+        elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
+            self.next_take(reverse)
+        elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
+            self.next_Feed(reverse)
 
     def safe_check_columns(self):
         return True

CU/Feeding_C.py

@@ -33,6 +33,13 @@ class FeedStatus(IntEnum):
     FReverse = 11
     FStartReverse = 12
 
+class LineType(Enum):
+    Straight = 0
+    CureStart = 1
+    CureMid = 2
+    CureEnd = 3
+
+
 class FeedMidStatus(Enum):
     FMid_Start = 1
     FMid_Take = 2
@@ -374,7 +381,7 @@ class Feeding:
         log.log_message(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
         pass
 
-    def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=Constant.speed):
+    def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=Constant.speed,real_position1=None):
         position_instruction = Instruction()
         position_instruction.speed = speed
         position_instruction.m0 = real_position.X
@@ -383,7 +390,15 @@ class Feeding:
         position_instruction.m3 = real_position.U
         position_instruction.m4 = real_position.V
         position_instruction.m5 = real_position.W
+
         position_instruction.action = move_type.value
+        if position_instruction.action == 17:
+            position_instruction.m0_p = real_position1.X
+            position_instruction.m1_p = real_position1.Y
+            position_instruction.m2_p = real_position1.Z
+            position_instruction.m3_p = real_position1.U
+            position_instruction.m4_p = real_position1.V
+            position_instruction.m5_p = real_position1.W
         instruction_command = CMDInstructRequest()
         instruction_command.instructions.append(position_instruction)
         request_command = instruction_command.toString()
@@ -408,19 +423,31 @@ class Feeding:
     def next_start(self,reverse=False):
         start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
         self.feedStatus = start_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-        self.sendTargPosition(start_pos.position)
+        if start_pos.lineType == LineType.CureMid:
+            start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
+            self.sendTargPosition(real_position=start_pos.position, move_type=MoveType.Cure, real_position1=start_pos1.position)
+        else:
+            self.sendTargPosition(start_pos.position)
         pass
 
     def next_take(self,reverse=False):
         take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
         self.feedStatus = take_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-        self.sendTargPosition(take_pos.position)
+        if take_pos.lineType == LineType.CureMid:
+            take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
+            self.sendTargPosition(real_position=take_pos.position, move_type=MoveType.Cure, real_position1=take_pos1.position)
+        else:
+            self.sendTargPosition(take_pos.position)
         pass
 
     def next_Feed(self,reverse=False):
         feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
         self.feedStatus = feed_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-        self.sendTargPosition(feed_pos.position)
+        if feed_pos.lineType == LineType.CureMid:
+            feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
+            self.sendTargPosition(real_position=feed_pos.position, move_type=MoveType.Cure, real_position1=feed_pos1.position)
+        else:
+            self.sendTargPosition(feed_pos.position)
 
     def get_current_position(self):
         if self.feed_Mid_Status == FeedMidStatus.FMid_Start: