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
2024-10-11 23:05:08 +08:00
parent 47f8d81b7f
commit 2af2bff5e8
6 changed files with 142 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
CU/Feeding.py
View File

@ -21,15 +21,21 @@ class FeedStatus(IntEnum):
FSafeF = 6
FFeedP = 7
FBroken = 8
FFinished = 9
FCut = 9
FShake = 10
FDropBag = 11
FFinished = 12
class FeedLine:
def __init__(self,name,safe_position:Real_Position,photo_position:Real_Position,mid_position:Real_Position,broken_position:Real_Position,feed_position:Real_Position):
def __init__(self,name,safe_position:Real_Position,photo_position:Real_Position,mid_position:Real_Position,broken1_position:Real_Position,broken2_position:Real_Position,drop_bag_position:Real_Position,zip_bag_position:Real_Position,feed_position:Real_Position):
self.safe_position = safe_position
self.photo_position = photo_position
self.feed_position = feed_position
self.mid_position = mid_position
self.broken_position = broken_position
self.broken1_position = broken1_position
self.broken2_position = broken2_position
self.drop_bag_position = drop_bag_position
self.zip_bag_position = zip_bag_position
self.take_position = None
self.name = name
@ -150,35 +156,72 @@ class Feeding():
log.log_message(logging.INFO, Constant.str_feed_feed)
if self.feedConfig.feedLine.feed_position.compare(real_position):
self.feedStatus = FeedStatus.FBroken
self.sendTargPosition(self.feedConfig.feedLine.broken_position)
self.sendTargPosition(self.feedConfig.feedLine.broken1_position)
pass #破袋
elif self.feedStatus==FeedStatus.FBroken:
log.log_message(logging.INFO, Constant.str_feed_broken)
if self.feedConfig.feedLine.broken_position.compare(real_position):
self.feedConfig.num = self.feedConfig.num-1
if self.feedConfig.num == 0:
self.feedStatus=FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.init_detection_image()
log.log_message(logging.INFO, Constant.str_feed_finish)
if self.feedConfig.feedLine.broken1_position.compare(real_position):
self.sendTargPosition(self.feedConfig.feedLine.broken2_position)
if self.feedConfig.feedLine.broken2_position.compare(real_position):
current_position = Real_Position().init_position(0,0,0,0,0,0)
current_position.X = self.robotClient.status_model.axis_0
current_position.Y = self.robotClient.status_model.axis_1
current_position.Z = self.robotClient.status_model.axis_2
current_position.U = self.robotClient.status_model.axis_3
current_position.V = self.robotClient.status_model.axis_4
current_position.W = self.robotClient.status_model.axis_5
for i in range(8):
if (i % 2 == 0) ^ (i >= 3): ## 过一半取消
c1 = current_position.U - 5
self.sendTargPosition(c1, MoveType.AXIS, speed=Constant.shake_speed)
else:
c1 = current_position.U + 5
self.sendTargPosition(c1, MoveType.AXIS, speed=Constant.shake_speed)
if True: # 延迟判断，如果最后点位延迟1s，则认为阶段完成
self.feedStatus = FeedStatus.FDropBag
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
elif self.feedStatus == FeedStatus.FDropBag:
log.log_message(logging.INFO, Constant.str_feed_drop)
if self.feedConfig.feedLine.safe_position.compare(real_position):
if self.feedConfig.num - 1 != 0:
self.sendTargPosition(self.feedConfig.feedLine.drop_bag_position)
else:
log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
self.feedStatus = FeedStatus.FSafeP
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.sendTargPosition(self.feedConfig.feedLine.zip_bag_position)
pass
if self.feedConfig.feedLine.drop_bag_position.compare(real_position):
# TODO 松开吸嘴
self.feedConfig.num = self.feedConfig.num - 1
log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
self.feedStatus = FeedStatus.FSafeP
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
if self.feedConfig.feedLine.zip_bag_position.compare(real_position):
log.log_message(logging.INFO, Constant.str_feed_zip_bag)
# TODO 松开吸嘴 是否需要判断放没放开不
self.feedStatus = FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.init_detection_image()
log.log_message(logging.INFO, Constant.str_feed_finish)
pass
def sendTargPosition(self,real_position):
def sendTargPosition(self,real_position,move_type:MoveType=MoveType.WORLD ,speed = Constant.speed):
from Model.RobotModel import Instruction
position_instruction = Instruction()
position_instruction.speed = Constant.speed
position_instruction.speed = speed
position_instruction.m0 = real_position.X
position_instruction.m1 = real_position.Y
position_instruction.m2 = real_position.Z
position_instruction.m3 = real_position.U
position_instruction.m4 = real_position.V
position_instruction.m5 = real_position.W
position_instruction.action = MoveType.WORLD.value
position_instruction.action = move_type.value
instruction_command = CMDInstructRequest()
instruction_command.instructions.append(position_instruction)
request_command = instruction_command.toString()
@ -196,6 +239,8 @@ class Feeding():
self.robotClient.add_sendQuene(request_command)
pass
def safe_check_columns(self):
return True
pass