update 完成所有投料逻辑，IO点位控制，急停控制
This commit is contained in:
FrankCV2048
100
CU/Feeding.py
100
CU/Feeding.py
@ -42,14 +42,18 @@ class FeedingConfig:
|
||||
def __init__(self, num:int, feedLine:FeedLine,photo_locs):
|
||||
self.num = num
|
||||
self.feedLine = feedLine
|
||||
self.photo_locs = photo_locs
|
||||
self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
|
||||
|
||||
def deal_photo_locs(self,photo_loc):
|
||||
position_photo = Real_Position()
|
||||
position_photo.init_position(photo_loc[0], photo_loc[1], photo_loc[2], photo_loc[3], photo_loc[4], photo_loc[5])
|
||||
return position_photo
|
||||
|
||||
def get_line_info(self):
|
||||
pass
|
||||
|
||||
|
||||
class Feeding:
|
||||
class Feeding :
|
||||
def __init__(self,robotClient:RobotClient,detection:Detection):
|
||||
self.feedConfig = None
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
@ -93,53 +97,79 @@ class Feeding:
|
||||
log.log_message(logging.INFO, Constant.str_feed_check)
|
||||
# 1, 检查是否是三列
|
||||
# 2, 检查是否有人
|
||||
if self.safe_check_columns() and self.safe_check_person():
|
||||
pass
|
||||
else:
|
||||
if self.feedConfig.num != 0:
|
||||
self.feedStatus = FeedStatus.FSafeP
|
||||
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
|
||||
# if self.safe_check_columns() and self.safe_check_person():
|
||||
# pass
|
||||
# else:
|
||||
if self.feedConfig.num != 0:
|
||||
self.feedStatus = FeedStatus.FSafeP
|
||||
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
|
||||
|
||||
|
||||
elif self.feedStatus==FeedStatus.FSafeP:
|
||||
log.log_message(logging.INFO,Constant.str_feed_safe)
|
||||
if self.feedConfig.feedLine.safe_position.compare(real_position):
|
||||
self.feedStatus = FeedStatus.FPhoto
|
||||
self.sendTargPosition(self.feedConfig.feedLine.photo_position)
|
||||
#self.sendTargPosition(self.feedConfig.feedLine.photo_position)
|
||||
#判断是哪一个问题
|
||||
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
log.log_message(logging.INFO, Constant.str_feed_photo)
|
||||
if self.feedConfig.feedLine.photo_position.compare(real_position):
|
||||
detect_pos_list = []
|
||||
for pos in self.feedConfig.photo_locs:
|
||||
self.sendTargPosition(pos)
|
||||
while not pos.compare(real_position): #可以优化 TODO
|
||||
time.sleep(0.1)
|
||||
code, img, xyz, uvw, mng = self.detection.get_position() #检测结果
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto)
|
||||
self.detection_image = img
|
||||
if xyz!=None:
|
||||
if xyz != None:
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
|
||||
#dp = Detection_Position().init_position(*xyz, *uvw)
|
||||
from Trace.handeye_calibration import R_matrix,getPosition
|
||||
# dp = Detection_Position().init_position(*xyz, *uvw)
|
||||
from Trace.handeye_calibration import R_matrix, getPosition
|
||||
rotation = R_matrix(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2-10,
|
||||
self.robotClient.status_model.world_2 - 10, #TODO 这个10 需要确定
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
|
||||
# 黄老师给我的xyz和法向量
|
||||
target_position, noraml_base = getPosition(*xyz, *uvw,rotation,*mng)
|
||||
log.log_message(logging.INFO, Constant.str_feed_covert_success)
|
||||
self.feedConfig.feedLine.take_position = Real_Position().init_position(*target_position[:3],*noraml_base)
|
||||
self.feedStatus = FeedStatus.FTake
|
||||
self.sendTargPosition(self.feedConfig.feedLine.take_position)
|
||||
target_position, noraml_base = getPosition(*xyz, *uvw, rotation, *mng)
|
||||
detect_pos_list.append(Real_Position().init_position(*target_position[:3],*noraml_base))
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
|
||||
else:
|
||||
log.log_message(logging.ERROR,Constant.str_feed_takePhoto_fail)
|
||||
print("打印日志，保存失败图像")
|
||||
|
||||
log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail+real_position.to_string())
|
||||
z_diff, max_z_index = (lambda pts: (
|
||||
max(pts, key=lambda p: p.Z).Z - min(pts, key=lambda p: p.Z).Z,
|
||||
pts.index(max(pts, key=lambda p: p.Z))
|
||||
))(detect_pos_list)
|
||||
if len(self.feedConfig.photo_locs) == 5:
|
||||
if z_diff < Constant.bag_height and len(detect_pos_list)==3: # 第一次检测到没有高度差距开始三列拍照 TODO 保留全部的开关
|
||||
# 拍照位置从五个变为三个
|
||||
self.feedConfig.photo_locs = [detect_pos_list[0],detect_pos_list[2],detect_pos_list[4]]
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[0]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_front_finish)
|
||||
else:
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_front)
|
||||
else:
|
||||
if z_diff < Constant.bag_height:
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[0]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_new_line)
|
||||
else:
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_line)
|
||||
self.feedStatus = FeedStatus.FTake
|
||||
self.sendTargPosition(self.feedConfig.feedLine.take_position)
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_move)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FTake:
|
||||
log.log_message(logging.INFO, Constant.str_feed_take)
|
||||
if self.feedConfig.feedLine.take_position != None and self.feedConfig.feedLine.take_position.compare(real_position):
|
||||
# 打开吸嘴并返回 #TODO
|
||||
# 打开吸嘴并返回
|
||||
self.sendIOControl(self.robotClient.con_ios[0],1)
|
||||
self.sendIOControl(self.robotClient.con_ios[1],1)
|
||||
self.sendIOControl(self.robotClient.con_ios[2],1)
|
||||
# TODO 检测是否通 不然报警
|
||||
time.sleep(2)
|
||||
self.feedStatus = FeedStatus.FSafeF
|
||||
log.log_message(logging.INFO, Constant.str_feed_take_success)
|
||||
@ -178,7 +208,10 @@ class Feeding:
|
||||
self.sendTargPosition(self.feedConfig.feedLine.drop_bag_position)
|
||||
pass
|
||||
if self.feedConfig.feedLine.drop_bag_position.compare(real_position):
|
||||
# TODO 松开吸嘴
|
||||
self.sendIOControl(self.robotClient.con_ios[0],0)
|
||||
self.sendIOControl(self.robotClient.con_ios[1],0)
|
||||
self.sendIOControl(self.robotClient.con_ios[2],0)
|
||||
# TODO 检测是否断 不然报警
|
||||
time.sleep(2)
|
||||
self.feedConfig.num = self.feedConfig.num - 1
|
||||
log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
|
||||
@ -192,6 +225,23 @@ class Feeding:
|
||||
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
|
||||
|
||||
|
||||
def send_emergency_sound(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 1)
|
||||
|
||||
def send_emergency_stop(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 0)
|
||||
|
||||
def sendIOControl(self,IO_bit,IO_Status:int):
|
||||
from Model.RobotModel import Instruction
|
||||
IO_command = CMDInstructRequest()
|
||||
io_instruction = Instruction()
|
||||
io_instruction.IO = True
|
||||
io_instruction.io_status = IO_Status
|
||||
io_instruction.point = IO_bit # {"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]}
|
||||
IO_command.dsID = 'HCRemoteCommand'
|
||||
IO_command.instructions.append(io_instruction)
|
||||
self.robotClient.add_sendQuene(IO_command)
|
||||
pass
|
||||
|
||||
def sendTargPosition(self,real_position,move_type:MoveType=MoveType.WORLD ,speed = Constant.speed):
|
||||
from Model.RobotModel import Instruction
|
||||
|
||||
Reference in New Issue
Block a user