update 重新规划架构，绘制机械臂夹爪轨迹图

CU/Feeding_C.py

@@ -39,26 +39,57 @@ class FeedPosition:
 class FeedLine:
     def __init__(self, id, name, feed_positions:list):
         self.feed_positions = feed_positions
-        self.pos_index = 0
+        self.feeding2end_pos_index = 0
+        self.origin2start_pos_index = 0
+        self.start2take_pos_index = 0
         self.name = name
         self.id = id
+        self.get_position_list()
 
     def get_current_position(self):
-        pos = self.feed_positions[self.pos_index]
+        pos = self.feeding_to_end[self.feeding2end_pos_index]
         return pos
-    def get_next_position(self):
-        pos = self.feed_positions[self.pos_index]
-        self.pos_index += 1
+    def get_next_feed_position(self):
+        pos = self.feeding_to_end[self.feeding2end_pos_index]
+        self.feeding2end_pos_index += 1
         return pos
+
+    def get_next_start_position(self):
+        pos = self.origin2start_pos_index[self.origin2start_pos_index]
+        self.origin2start_pos_index += 1
+        return pos
+
+    def get_next_take_position(self):
+        pos = self.start2take_pos_index[self.start2take_pos_index]
+        self.start2take_pos_index += 1
+        return pos
+
     def get_take_position(self):
-        for i in range(len(self.feed_positions)):
-            if self.feed_positions[i].status == FeedStatus.FTake:
-                return self.feed_positions[i]
+        for i in range(len(self.feeding_to_end)):
+            if self.feeding_to_end[i].status == FeedStatus.FTake:
+                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:
+                self.feeding_to_end[i].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.FStart:
+                index_start = i
+                break
         for i in range(len(self.feed_positions)):
             if self.feed_positions[i].status == FeedStatus.FTake:
-                self.feed_positions[i].position = position
+                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):
@@ -310,6 +341,7 @@ class Feeding:
         self.robotClient.add_sendQuene(request_command)
         pass
 
+    def
     def next_target(self):
         feed_pos = self.feedConfig.feedLine.get_next_position()
         self.feedStatus = feed_pos.status