update 补充部分

CU/Catch.py

@@ -1,20 +1,57 @@
 from enum import Enum
 
+from COM.COM_Robot import RobotClient
+from Util.util_time import CClockPulse, CTon
+
 
 class CatchStatus(Enum):
     CNone = 0
     CTake = 1
-    CPress = 2
+    CDrop = 2
     CShake = 3
+    COk = 4
 
 class catch:
-    def __init__(self, x, y, width, height, angle, color):
-        pass
-
+    def __init__(self, robotClient: RobotClient):
+        self.robotClient = robotClient
+        self.catch_status = CatchStatus.CNone
+        self.shake_continue = CTon()
+        self.shakePulse = CClockPulse()
+        self.shake_count = 5
     def run(self):
+        if self.catch_status == CatchStatus.CNone:
+            return
 
-        pass
+        if self.catch_status == CatchStatus.CTake:
+            self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
+            if self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
+                self.catch_status = CatchStatus.COk
+
+        if self.catch_status == CatchStatus.CDrop:
+            self.robotClient.sendIOControl(self.robotClient.con_ios[0],0)
+            self.robotClient.sendIOControl(self.robotClient.con_ios[1],1)
+            if self.robotClient.check_outputQ(self.robotClient.con_ios[1]) and not self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
+                self.catch_status = CatchStatus.COk
+        if self.catch_status == CatchStatus.CShake:
+            if not self.shake_continue.Q(True, 3000):
+                shakeQ = self.shakePulse.Q(True, 1000, 1000)
+                self.robotClient.sendIOControl(self.robotClient.con_ios[2], shakeQ)
+            else:
+                self.shake_continue.SetReset()
+                self.catch_status = CatchStatus.COk
+                if self.robotClient.check_outputQ(self.robotClient.con_ios[2]):
+                    self.robotClient.sendIOControl(self.robotClient.con_ios[2], False)
+
+        if self.catch_status == CatchStatus.COk:
+
+            pass
 
 
     def take_bag(self):
+        return True
+
+    def press_bag(self):
+        return True
+
+    def shake_bag(self):
         return True