99 lines
3.8 KiB
Python
99 lines
3.8 KiB
Python
|
#!/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)
|
|||
|
|
print('抓料成功真实抓料')
|
|||
|
|
# 网络继电器
|
|||
|
|
open(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)
|
|||
|
|
# 网络继电器
|
|||
|
|
close(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')
|
|||
|
|
close(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
|