60 lines
2.1 KiB
Python
60 lines
2.1 KiB
Python
from enum import Enum
|
|
|
|
import Constant
|
|
from COM.COM_Robot import RobotClient
|
|
from Util.util_time import CClockPulse, CTon
|
|
|
|
|
|
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.shakePulse = CClockPulse()
|
|
self.shake_count = 5
|
|
def run(self):
|
|
if self.catch_status == CatchStatus.CNone:
|
|
return
|
|
|
|
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]) or Constant.Debug:
|
|
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 Constant.Debug or 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, 1500):
|
|
shakeQ = self.shakePulse.Q(True, 50, 50)
|
|
self.robotClient.sendIOControl(self.robotClient.con_ios[2], 1 if shakeQ else 0)
|
|
print("正在震动" if shakeQ else "震动结束")
|
|
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)
|
|
print("震动结束")
|
|
|
|
if self.catch_status == CatchStatus.COk:
|
|
self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)
|
|
pass
|
|
|
|
|
|
def take_bag(self):
|
|
return True
|
|
|
|
def press_bag(self):
|
|
return True
|
|
|
|
def shake_bag(self):
|
|
return True |