xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🚑 update 更新抓取和扔料

Browse Source
This commit is contained in:
FrankCV2048
2025-01-14 23:02:14 +08:00
parent 3e065af6a0
commit ecfd9551e1
9 changed files with 1772 additions and 15181 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
CU/Catch.py
View File

@ -1,3 +1,4 @@
import time
from enum import Enum
import Constant
@ -17,27 +18,46 @@ class Catch:
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
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:
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 not self.is_send_take_command:
self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
self.is_send_take_command = True
else:
if self.take_continue.Q(True,self.robotClient.time_delay_take*1000):
self.is_send_take_command = False
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 not self.is_send_command:
self.robotClient.sendIOControl(self.robotClient.con_ios[0], 0)
for _ in range(self.drop_count):
self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1, delay=self.robotClient.time_delay_put)
self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)
self.robotClient.sendIOControl(self.robotClient.con_ios[1], 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:
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 "震动结束")
if not self.shake_continue.Q(True, 600): # 1500
self.shake_Q = not self.shake_Q # 10
self.robotClient.sendIOControl(self.robotClient.con_ios[2], 1 if self.shake_Q else 0)
else:
self.shake_continue.SetReset()
self.catch_status = CatchStatus.COk
@ -46,7 +66,7 @@ class Catch:
print("震动结束")
if self.catch_status == CatchStatus.COk:
self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)
self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0,emptyList='1')
pass