first commit

CU/Catch.py

@@ -0,0 +1,98 @@
+#!/usr/bin/python3
+import time
+from enum import Enum
+
+import Constant
+from COM.COM_Robot import RobotClient
+from Util.util_time import CClockPulse, CTon
+from .EMV import *
+
+class CatchStatus(Enum):
+    CNone = 0
+    CTake = 1
+    CDrop = 2
+    CShake = 3
+    COk = 4
+
+class Catch:
+    def __init__(self, robotClient: RobotClient):
+        self.robotClient = robotClient
+        self.catch_status = CatchStatus.CNone
+        self.shake_continue = CTon()
+        self.drop_continue = CTon()
+        self.take_continue = CTon()
+        self.shakePulse = CClockPulse()
+        self.shake_count = 5
+        self.drop_count = 2
+        self.is_send_command = False#这里到时候可以改成True，然后加一个获取继电器状态变成False
+        self.is_send_take_command = False
+        self.shake_Q = False
+    def run(self):
+        if self.catch_status == CatchStatus.CNone:
+            return
+
+        if self.catch_status == CatchStatus.CTake:
+            # 这里可以添加继电器状态获取，把is_send_command = False
+            if not self.is_send_take_command:
+                # 本身IO
+                #self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
+                # 网络继电器
+                close(1, 0, 0) #逻辑关闭电磁阀1
+                self.is_send_take_command = True
+
+        if self.catch_status == CatchStatus.CDrop:
+            if not self.is_send_command:
+                # 本身IO
+                # self.robotClient.sendIOControl(self.robotClient.con_ios[0], 0)
+                # 网络继电器
+                open(1, 0, 0)#逻辑
+                #time.sleep(1)
+                #for _ in range(self.drop_count):
+                    # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1, delay=self.robotClient.time_delay_put)
+                    #open(0, 0, 1)
+                    #time.sleep(self.robotClient.time_delay_put) # 会造成这个时间点 其他命令插入不进去 需要另开线程
+                    #close(0, 0, 1)
+                    # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)
+
+                # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1)
+                #close(0, 0, 1)
+                self.is_send_command = True
+            if self.drop_continue.Q(True,self.robotClient.time_delay_put*1000*self.drop_count):
+                # if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[1]) and not self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
+                self.is_send_command = False
+                self.catch_status = CatchStatus.COk
+                self.drop_continue.SetReset()
+
+        if self.catch_status == CatchStatus.CShake:     # 1500
+            self.shake_Q = not self.shake_Q   #   10
+            if not self.shake_continue.Q(True, 6000):
+            #if self.shake_Q:
+                    #open(0, 1, 0)
+                #else:
+                    #close(0, 1, 0)
+            #else:
+                self.shake_continue.SetReset()
+                #self.catch_status = CatchStatus.COk
+                #if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[2]):
+                # self.robotClient.sendIOControl(self.robotClient.con_ios[2], 0)
+                #close(0, 1, 0)
+                print("震动结束")
+
+        if self.catch_status == CatchStatus.COk:
+            self.shake_continue.SetReset()
+            # self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0,emptyList='1')
+            open(1,0,0)#逻辑
+            #close(0, 1, 0)
+            #close(0, 0, 1)
+            self.is_send_take_command = False
+            pass
+
+
+    def take_bag(self):
+        return True
+
+    def press_bag(self):
+        return True
+
+    def shake_bag(self):
+        return True