复位-急停
This commit is contained in:
134
CU/Feeding.py
134
CU/Feeding.py
@ -36,9 +36,16 @@ from Mv3D.calculate_diff2 import calculate_offset_from_image
|
||||
|
||||
class ResetStatus(Enum):
|
||||
RNone = 0
|
||||
#开始复位
|
||||
RStart = 1
|
||||
#复位向上移到最高点
|
||||
RRunging = 2
|
||||
#扔包点
|
||||
ROk =3
|
||||
#复位点
|
||||
RRestPoint = 5
|
||||
#复位完成
|
||||
RFinish = 4
|
||||
|
||||
class FeedStatus(IntEnum):
|
||||
FNone = 0
|
||||
@ -325,6 +332,8 @@ class Feeding(QObject):
|
||||
stack_finish_signal=Signal()
|
||||
#误差过大，通知用户
|
||||
feed_error_signal=Signal(int,str)
|
||||
#数量保存信号
|
||||
feed_nums_signal=Signal(int)
|
||||
|
||||
def __init__(self, robotClient: RobotClient,relay_controller:RelayController):
|
||||
super().__init__()
|
||||
@ -496,6 +505,10 @@ class Feeding(QObject):
|
||||
#self.catch.catch_status = CatchStatus.CTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
|
||||
if not real_position.compare(self.robotClient.origin_position,is_action=True):
|
||||
return
|
||||
|
||||
#码垛的数量和配置的数量一致时
|
||||
if self.feedConfig.remain_count >=self.feedConfig.num:
|
||||
#关闭，暂停
|
||||
@ -506,7 +519,7 @@ class Feeding(QObject):
|
||||
self.feedConfig.feedLine.origin2start_pos_index = len(self.feedConfig.feedLine.origin_to_start) - 2
|
||||
#码垛数量重置
|
||||
self.feedConfig.remain_count=0
|
||||
self.next_position(self.is_reverse)
|
||||
# self.next_position(self.is_reverse)
|
||||
#码垛完成信号通知
|
||||
self.stack_finish_signal.emit()
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
|
||||
@ -521,6 +534,8 @@ class Feeding(QObject):
|
||||
return
|
||||
#初始点无论如何先打开夹爪
|
||||
self.relay_controller.close(clamp=True)
|
||||
|
||||
self.relay_controller.close(blow_sensor2=True)
|
||||
#重新抓去信号料带
|
||||
self.take_sensor_signal=False
|
||||
self.relay_controller.sensor2_ready=True
|
||||
@ -570,10 +585,11 @@ class Feeding(QObject):
|
||||
print(f"视频模型异常：{loc_image_path}",e)
|
||||
# loc_is_next=False
|
||||
loc_offsetX=0
|
||||
self.feed_error_signal.emit(2,"视频模型异常")
|
||||
self.log_signal.emit(logging.ERROR, f"视频模型异常：{e}")
|
||||
if not Constant.DebugPosition:
|
||||
self.feed_error_signal.emit(2,"视频模型异常")
|
||||
self.log_signal.emit(logging.ERROR, f"视频模型异常：{e}")
|
||||
finally:
|
||||
if loc_is_next:
|
||||
if loc_is_next or Constant.DebugPosition:
|
||||
self.feedConfig.feedLine.set_feeding_to_end(loc_offsetX)
|
||||
self.next_position()
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing2)
|
||||
@ -677,9 +693,10 @@ class Feeding(QObject):
|
||||
# (后续增加) 视觉确认: 拍照确认袋子已放置
|
||||
# self.detection.get_position(...)
|
||||
# self.feedConfig.feedLine.set_take_position(...)
|
||||
|
||||
self.relay_controller.open(blow_sensor2=True)
|
||||
# 码垛数量增加
|
||||
self.feedConfig.remain_count = self.feedConfig.remain_count + 1
|
||||
self.feed_nums_signal.emit(self.feedConfig.remain_count)
|
||||
# self.feedConfig.num = self.feedConfig.num - 1
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.remain_count}')
|
||||
# 5. *** 关键步骤 ***: 移动到路径中的下一个点
|
||||
@ -909,6 +926,7 @@ class Feeding(QObject):
|
||||
print(request_command)
|
||||
self.robotClient.add_sendQuene(request_command)
|
||||
pass
|
||||
|
||||
# def get_take_position(self):
|
||||
# if Constant.Debug:
|
||||
# return self.robotClient.status_model.getRealPosition()
|
||||
@ -1033,3 +1051,109 @@ class Feeding(QObject):
|
||||
return True
|
||||
pass
|
||||
|
||||
class FeedReset(QObject):
|
||||
def __init__(self,robotClient: RobotClient,relay_controller:RelayController,positions):
|
||||
super().__init__()
|
||||
self.robotClient = robotClient
|
||||
self.relay_controller = relay_controller
|
||||
self.reversed_positions = positions
|
||||
self.reset_status = ResetStatus.RNone
|
||||
|
||||
def run_reset(self):
|
||||
if self.reset_status == ResetStatus.RNone:
|
||||
return
|
||||
|
||||
real_position = Real_Position()
|
||||
real_position.init_position_joint_and_world(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5,
|
||||
self.robotClient.status_model.axis_0,
|
||||
self.robotClient.status_model.axis_1,
|
||||
self.robotClient.status_model.axis_2,
|
||||
self.robotClient.status_model.axis_3,
|
||||
self.robotClient.status_model.axis_4,
|
||||
self.robotClient.status_model.axis_5)
|
||||
|
||||
if self.reset_status == ResetStatus.RStart:
|
||||
#当前位置
|
||||
self.reverse_index = 0
|
||||
self.current_position = PositionModel()
|
||||
self.current_position.init_position(real_position)
|
||||
self.reset_status = ResetStatus.RRunging
|
||||
|
||||
elif self.reset_status == ResetStatus.RRunging:
|
||||
#到达当前位置-》移到机械臂最上方
|
||||
|
||||
if not real_position.compare(self.current_position.get_position(),is_action=True):
|
||||
return
|
||||
self.current_position.X=real_position.Axis_0
|
||||
self.current_position.Y=-59.997
|
||||
self.current_position.Z=real_position.Axis_2
|
||||
self.current_position.U=real_position.Axis_3
|
||||
self.current_position.V=real_position.Axis_4
|
||||
self.current_position.W=real_position.Axis_5
|
||||
self.current_position.lineType=4
|
||||
# pos_model.get_position().Y = pos_model.get_position().Y + 10000
|
||||
#linetype 4是关节移动
|
||||
self.sendTargPosition(pos_model= self.current_position,speed=60)
|
||||
self.reset_status=ResetStatus.ROk
|
||||
|
||||
elif self.reset_status == ResetStatus.ROk:
|
||||
#到达机械臂最上方--》扔包点
|
||||
if not real_position.compare(self.current_position.get_position(),is_action=True):
|
||||
return
|
||||
pos_model=self.reversed_positions[self.reverse_index]
|
||||
|
||||
self.current_position = pos_model
|
||||
self.sendTargPosition(pos_model=pos_model,speed=60)
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
self.reset_status = ResetStatus.RRestPoint
|
||||
elif self.reset_status == ResetStatus.RRestPoint:
|
||||
#到达扔包点-》发送复位点
|
||||
if real_position.compare(self.current_position.get_position(),is_action=True):
|
||||
self.relay_controller.close(clamp=True)
|
||||
pos_model=self.reversed_positions[self.reverse_index]
|
||||
self.current_position = pos_model
|
||||
self.sendTargPosition(pos_model=pos_model,speed=60)
|
||||
self.reset_status = ResetStatus.RFinish
|
||||
elif self.reset_status == ResetStatus.RFinish:
|
||||
#到达复位点-》RNone
|
||||
if not real_position.compare(self.current_position.get_position(),is_action=True):
|
||||
return
|
||||
self.reset_status = ResetStatus.RNone
|
||||
|
||||
|
||||
def sendTargPosition(self, pos_model:PositionModel, speed=5):
|
||||
real_position=pos_model.get_position()
|
||||
position_instruction = Instruction()
|
||||
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.smooth = self.robotClient.smooth
|
||||
|
||||
if pos_model.lineType == 4:
|
||||
#linetype 4是关节移动
|
||||
position_instruction.action = MoveType.AXIS.value
|
||||
else:
|
||||
position_instruction.action = MoveType.WORLD.value
|
||||
|
||||
if position_instruction.action == 17:
|
||||
position_instruction.m0_p = real_position1.X
|
||||
position_instruction.m1_p = real_position1.Y
|
||||
position_instruction.m2_p = real_position1.Z
|
||||
position_instruction.m3_p = real_position1.U
|
||||
position_instruction.m4_p = real_position1.V
|
||||
position_instruction.m5_p = real_position1.W
|
||||
instruction_command = CMDInstructRequest()
|
||||
instruction_command.instructions.append(position_instruction)
|
||||
request_command = instruction_command.toString()
|
||||
self.robotClient.add_sendQuene(request_command)
|
||||
pass
|
||||
|
||||
|
||||
Reference in New Issue
Block a user