🚑 update 更新抓取和扔料
This commit is contained in:
FrankCV2048
44
CU/Catch.py
44
CU/Catch.py
@ -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
|
||||
|
||||
|
||||
|
||||
@ -208,6 +208,7 @@ class Feeding(QObject):
|
||||
self.init_detection_image()
|
||||
self.pause = False
|
||||
self.cRis_photo = CRisOrFall()
|
||||
self.cRis_shake = CRisOrFall()
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
self.is_reverse = False
|
||||
# 复位集合
|
||||
@ -316,9 +317,10 @@ class Feeding(QObject):
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FMid:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
|
||||
|
||||
feed_pos = self.get_current_position(self.is_reverse)
|
||||
if feed_pos.get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
|
||||
@ -450,7 +452,7 @@ class Feeding(QObject):
|
||||
if self.catch.catch_status == CatchStatus.COk :
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
#self.feedConfig.feedLine.set_take_position(None)
|
||||
time.sleep(self.robotClient.time_delay_take)
|
||||
# time.sleep(self.robotClient.time_delay_take)
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
|
||||
self.next_position()
|
||||
|
||||
@ -466,21 +468,23 @@ class Feeding(QObject):
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken1:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken2:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FShake:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position,is_action=True):
|
||||
# TODO 震动方案
|
||||
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
|
||||
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
self.catch.catch_status = CatchStatus.CShake
|
||||
@ -489,7 +493,6 @@ class Feeding(QObject):
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.COk:
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
time.sleep(self.robotClient.time_delay_shake)
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropBag:
|
||||
@ -507,7 +510,7 @@ class Feeding(QObject):
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.COk:
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
time.sleep(self.robotClient.time_delay_put)
|
||||
# time.sleep(self.robotClient.time_delay_put)
|
||||
# self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30, save_img_point=0, Height_reduce=30, width_reduce=30)
|
||||
# self.feedConfig.feedLine.set_take_position(self.get_take_position())
|
||||
if self.detect.detect_status == DetectStatus.DNone:
|
||||
@ -587,9 +590,6 @@ class Feeding(QObject):
|
||||
self.reset_status = ResetStatus.RNone
|
||||
self.send_emergency_stop()
|
||||
|
||||
|
||||
|
||||
|
||||
def return_original_position(self):
|
||||
|
||||
self.run_reverse = True
|
||||
@ -672,6 +672,10 @@ class Feeding(QObject):
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
|
||||
pass
|
||||
|
||||
def sendTargPosition1(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
|
||||
pass
|
||||
|
||||
|
||||
def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
|
||||
position_instruction = Instruction()
|
||||
position_instruction.speed = speed
|
||||
@ -705,10 +709,12 @@ class Feeding(QObject):
|
||||
|
||||
|
||||
try:
|
||||
print('fuck1',log_str)
|
||||
self.log_signal.emit(logging.INFO, log_str)
|
||||
print('fuck2',log_str)
|
||||
except:
|
||||
return
|
||||
|
||||
print(request_command)
|
||||
self.robotClient.add_sendQuene(request_command)
|
||||
pass
|
||||
# def get_take_position(self):
|
||||
@ -778,6 +784,16 @@ class Feeding(QObject):
|
||||
move_type=MoveType.AXIS)
|
||||
|
||||
else:
|
||||
# if not reverse and self.feedStatus == FeedStatus.FShake:
|
||||
# if not reverse:
|
||||
# if self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, True):
|
||||
# pass
|
||||
# elif self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, False):
|
||||
# self.feedStatus = FeedStatus.FShake
|
||||
# self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
# return
|
||||
|
||||
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
|
||||
def get_current_position(self,is_reverse=False):
|
||||
|
||||
Reference in New Issue
Block a user