update 更新框架和控制细节

CU/Command.py

@@ -10,3 +10,7 @@ class Command:
         self.status = Status.Prepareing
         pass
 
+class FeedCommand(Command):
+    def __init__(self, feedingConfig):
+        super().__init__()
+        self.feed_config = feedingConfig

CU/Feeding.py

@@ -1,11 +1,22 @@
 from Model.Position import Real_Position
+from enum import Enum
+
+class FeedStatus(Enum):
+    FNone = 0
+    FStart = 1
+    FSafeP = 2
+    FPhoto = 3
+    FTake = 4
+    FSafeF = 5
+    FFeedP = 6
+    FFinished = 7
 
 class FeedLine:
     def __init__(self,safe_position:Real_Position,photo_position:Real_Position,feed_position:Real_Position):
         self.safe_position = safe_position
         self.photo_position = photo_position
         self.feed_position = feed_position
-
+        self.take_position = None
 
 
 class FeedingConfig:
@@ -19,5 +30,78 @@ class FeedingConfig:
 
 
 class Feeding():
-    def __init__(self,feedingConfig):
+    def __init__(self,robotClient:RobotClient):
+        self.feedConfig = None
+        self.feedStatus = FeedStatus.FNone
+        self.robotClient = robotClient
         pass
+
+    def run(self):
+        # 获取事件坐标
+        real_position = Real_Position()
+        real_position.init_position(self.robotClient.status_model.world_n[0],
+                                    self.robotClient.status_model.world_n[1],
+                                    self.robotClient.status_model.world_n[2],
+                                    self.robotClient.status_model.world_n[3],
+                                    self.robotClient.status_model.world_n[4],
+                                    self.robotClient.status_model.world_n[5],
+                                    self.robotClient.status_model.world_n[6]);
+
+        if self.feedStatus==FeedStatus.FNone:
+
+            pass
+        elif self.feedStatus==FeedStatus.FStart:
+            if self.feedConfig.num != 0:
+                self.feedStatus = FeedStatus.FSafeP
+
+                sendTargPosition(self.feedConfig.safe_position)
+                #print(request_command)
+
+            pass
+        elif self.feedStatus==FeedStatus.FSafeP:
+
+             if self.feedConfig.safe_position.compare(real_position):
+                self.feedStatus = FeedStatus.FPhoto
+                sendTargPosition(self.feedConfig.photo_position)
+
+        elif self.feedStatus == FeedStatus.FPhoto:
+            if self.feedConfig.photo_position.compare(real_position):
+                self.feedStatus = FeedStatus.FTake
+                pass # 发送拍照获取坐标 并 开始移动
+
+        elif self.feedStatus == FeedStatus.FTake:
+            if self.feedConfig.take_position != None and self.feedConfig.take_position.compare(real_position):
+                self.feedStatus = FeedStatus.FSafeF
+                pass #打开吸嘴并返回
+
+        elif self.feedStatus == FeedStatus.FSafeF:
+            if self.feedConfig.safe_position.compare(real_position):
+                self.feedStatus = FeedStatus.FFeedP
+                sendTargPosition(self.feedConfig.feed_position)
+                pass  #吸嘴开始
+
+
+        elif self.feedStatus==FeedStatus.FFeedP:
+            if self.feedConfig.feed_position.compare(real_position):
+                self.feedConfig.num = self.feedConfig.num-1
+                if self.feedConfig.num == 0:
+                    self.feedStatus=FeedStatus.FNone
+                else:
+                    self.feedStatus = FeedStatus.FSafeP
+                    sendTargPosition(self.feedConfig.safe_position)
+                pass
+
+    def sendTargPosition(self,real_position):
+        position_instruction = Instruction()
+        position_instruction.m0 = self.feedConfig.safe_position.X
+        position_instruction.m1 = self.feedConfig.safe_position.Y
+        position_instruction.m2 = self.feedConfig.safe_position.Z
+        position_instruction.m3 = self.feedConfig.safe_position.U
+        position_instruction.m4 = self.feedConfig.safe_position.V
+        position_instruction.m5 = self.feedConfig.safe_position.W
+        instruction_command = CMDInstructRequest()
+        instruction_command.instructions.append(vars(position_instruction))
+        request_command = vars(instruction_command)
+        self.robotClient.add_sendQuene(request_command)
+        pass
+