forked from huangxin/ailai
程序正常退出（界面关闭后（投料启动前后），实现所有线程有序退出）
This commit is contained in:
554
CU/Feeding.py
554
CU/Feeding.py
@ -20,19 +20,23 @@ from enum import Enum, IntEnum
|
||||
from COM.COM_Robot import RobotClient, DetectType
|
||||
from Model.RobotModel import CMDInstructRequest, MoveType
|
||||
from Trace.handeye_calibration import getPosition
|
||||
from Trace.handeye_calibration import getxyz,getxyz1
|
||||
from Trace.handeye_calibration import getxyz, getxyz1
|
||||
from Util.util_math import get_distance
|
||||
from Util.util_time import CRisOrFall
|
||||
#from Vision.camera_coordinate_dete import Detection
|
||||
|
||||
# from Vision.camera_coordinate_dete import Detection
|
||||
from Util.util_log import log
|
||||
from Model.RobotModel import Instruction
|
||||
from EMV.EMV import RelayController
|
||||
from CU.drop import DropPositionManager
|
||||
|
||||
|
||||
class ResetStatus(Enum):
|
||||
RNone = 0
|
||||
RStart = 1
|
||||
RRunging = 2
|
||||
ROk =3
|
||||
ROk = 3
|
||||
|
||||
|
||||
class FeedStatus(IntEnum):
|
||||
FNone = 0
|
||||
@ -42,13 +46,14 @@ class FeedStatus(IntEnum):
|
||||
FPhoto = 4
|
||||
FTake = 5
|
||||
FBroken1 = 6
|
||||
FBroken2 =7
|
||||
FBroken2 = 7
|
||||
FShake = 8
|
||||
FDropBag = 9
|
||||
FFinished = 10
|
||||
FReverse = 11
|
||||
FStartReverse = 12
|
||||
|
||||
|
||||
class LineType(Enum):
|
||||
Straight = 0
|
||||
CureMid = 2
|
||||
@ -59,15 +64,17 @@ class LineType(Enum):
|
||||
class FeedMidStatus(Enum):
|
||||
FMid_Start = 1
|
||||
FMid_Take = 2
|
||||
FMid_Feed= 3
|
||||
FMid_Feed = 3
|
||||
|
||||
|
||||
class FeedPosition:
|
||||
def __init__(self,status:FeedStatus,position:Real_Position):
|
||||
def __init__(self, status: FeedStatus, position: Real_Position):
|
||||
self.status = status
|
||||
self.position = position
|
||||
|
||||
|
||||
class FeedLine:
|
||||
def __init__(self, id, name, feed_positions:list):
|
||||
def __init__(self, id, name, feed_positions: list):
|
||||
self.feed_positions = copy.deepcopy(feed_positions)
|
||||
self.feeding2end_pos_index = 0
|
||||
self.origin2start_pos_index = 0
|
||||
@ -75,21 +82,41 @@ class FeedLine:
|
||||
self.name = name
|
||||
self.id = id
|
||||
|
||||
# --- 新增：用于存储从 ini 文件读取的多个投料点坐标 ---
|
||||
# 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...]
|
||||
# --- 新增：用于存储从 ini 文件读取的多个投料点坐标 ---
|
||||
# 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...]
|
||||
self.drop_point_list = []
|
||||
|
||||
def get_current_feed_position(self,is_reverse):
|
||||
pos = self.feeding_to_end[ (self.feeding2end_pos_index+1)%len(self.feeding_to_end) if is_reverse else self.feeding2end_pos_index-1]
|
||||
return pos
|
||||
def get_current_take_position(self,is_reverse):
|
||||
pos = self.start_to_take[ (self.start2take_pos_index+1)%len(self.start_to_take) if is_reverse else self.start2take_pos_index-1]
|
||||
return pos
|
||||
def get_current_start_position(self,is_reverse):
|
||||
pos = self.origin_to_start[(self.origin2start_pos_index+1)%len(self.origin_to_start) if is_reverse else self.origin2start_pos_index-1]
|
||||
def get_current_feed_position(self, is_reverse):
|
||||
pos = self.feeding_to_end[
|
||||
(
|
||||
(self.feeding2end_pos_index + 1) % len(self.feeding_to_end)
|
||||
if is_reverse
|
||||
else self.feeding2end_pos_index - 1
|
||||
)
|
||||
]
|
||||
return pos
|
||||
|
||||
def get_next_feed_position(self,reverse:bool=False):
|
||||
def get_current_take_position(self, is_reverse):
|
||||
pos = self.start_to_take[
|
||||
(
|
||||
(self.start2take_pos_index + 1) % len(self.start_to_take)
|
||||
if is_reverse
|
||||
else self.start2take_pos_index - 1
|
||||
)
|
||||
]
|
||||
return pos
|
||||
|
||||
def get_current_start_position(self, is_reverse):
|
||||
pos = self.origin_to_start[
|
||||
(
|
||||
(self.origin2start_pos_index + 1) % len(self.origin_to_start)
|
||||
if is_reverse
|
||||
else self.origin2start_pos_index - 1
|
||||
)
|
||||
]
|
||||
return pos
|
||||
|
||||
def get_next_feed_position(self, reverse: bool = False):
|
||||
pos = self.feeding_to_end[self.feeding2end_pos_index]
|
||||
if reverse:
|
||||
self.feeding2end_pos_index -= 1
|
||||
@ -101,7 +128,7 @@ class FeedLine:
|
||||
self.feeding2end_pos_index = 0
|
||||
return pos
|
||||
|
||||
def get_next_start_position(self,reverse:bool=False):
|
||||
def get_next_start_position(self, reverse: bool = False):
|
||||
pos = self.origin_to_start[self.origin2start_pos_index]
|
||||
if reverse:
|
||||
self.origin2start_pos_index -= 1
|
||||
@ -114,7 +141,7 @@ class FeedLine:
|
||||
|
||||
return pos
|
||||
|
||||
def get_next_take_position(self,reverse:bool=False):
|
||||
def get_next_take_position(self, reverse: bool = False):
|
||||
pos = self.start_to_take[self.start2take_pos_index]
|
||||
if reverse:
|
||||
self.start2take_pos_index -= 1
|
||||
@ -125,6 +152,7 @@ class FeedLine:
|
||||
if self.start2take_pos_index >= len(self.start_to_take):
|
||||
self.start2take_pos_index = 0
|
||||
return pos
|
||||
|
||||
def get_take_position(self):
|
||||
for i in range(len(self.feeding_to_end)):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
@ -140,20 +168,21 @@ class FeedLine:
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
# 计算 XYZ 坐标
|
||||
# xyz = getxyz(position.X, position.Y, position.Z, position.a, position.b, position.c)
|
||||
xyz = getxyz1(position.X, position.Y, position.Z, position.a, position.b, position.c)
|
||||
xyz = getxyz1(
|
||||
position.X,
|
||||
position.Y,
|
||||
position.Z,
|
||||
position.a,
|
||||
position.b,
|
||||
position.c,
|
||||
)
|
||||
# 创建 before 和 after 位置
|
||||
befor_take_position = Real_Position().init_position(xyz[0],
|
||||
xyz[1],
|
||||
xyz[2],
|
||||
position.U,
|
||||
position.V,
|
||||
position.W)
|
||||
after_take_position = Real_Position().init_position(xyz[0],
|
||||
xyz[1],
|
||||
xyz[2],
|
||||
position.U,
|
||||
position.V,
|
||||
position.W)
|
||||
befor_take_position = Real_Position().init_position(
|
||||
xyz[0], xyz[1], xyz[2], position.U, position.V, position.W
|
||||
)
|
||||
after_take_position = Real_Position().init_position(
|
||||
xyz[0], xyz[1], xyz[2], position.U, position.V, position.W
|
||||
)
|
||||
# 安全检查索引
|
||||
if i > 0:
|
||||
self.feeding_to_end[i - 1].set_position(befor_take_position)
|
||||
@ -176,7 +205,8 @@ class FeedLine:
|
||||
# 直接设置当前点的位置
|
||||
self.feeding_to_end[i].set_position(position)
|
||||
print(
|
||||
f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})")
|
||||
f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})"
|
||||
)
|
||||
break # 假设只有一个丢包点
|
||||
|
||||
def get_position_list(self):
|
||||
@ -189,11 +219,11 @@ class FeedLine:
|
||||
if self.feed_positions[i].status == FeedStatus.FPhoto.value:
|
||||
index_take = i
|
||||
|
||||
self.origin_to_start = self.feed_positions[: index_start+1]
|
||||
self.start_to_take = self.feed_positions[index_start:index_take+1]
|
||||
self.origin_to_start = self.feed_positions[: index_start + 1]
|
||||
self.start_to_take = self.feed_positions[index_start : index_take + 1]
|
||||
self.feeding_to_end = self.feed_positions[index_take:]
|
||||
|
||||
for i in range(len(self.feeding_to_end)): #插入动态中间点
|
||||
for i in range(len(self.feeding_to_end)): # 插入动态中间点
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
befor_position_model = PositionModel()
|
||||
befor_position_model.init_position(None)
|
||||
@ -201,19 +231,26 @@ class FeedLine:
|
||||
after_position_model.init_position(None)
|
||||
|
||||
self.feeding_to_end.insert(i, befor_position_model)
|
||||
self.feeding_to_end.insert(i+2, after_position_model)
|
||||
self.feeding_to_end.insert(i + 2, after_position_model)
|
||||
break
|
||||
|
||||
|
||||
class FeedingConfig:
|
||||
def __init__(self, num: int, feedLine: FeedLine, photo_locs):
|
||||
self.num = num
|
||||
self.feedLine = feedLine
|
||||
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])
|
||||
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):
|
||||
@ -224,7 +261,8 @@ class Feeding(QObject):
|
||||
need_origin_signal = Signal(str)
|
||||
take_no_photo_sigal = Signal()
|
||||
update_detect_image = Signal(np.ndarray)
|
||||
log_signal = Signal(int,str)
|
||||
log_signal = Signal(int, str)
|
||||
|
||||
def __init__(self, robotClient: RobotClient):
|
||||
super().__init__()
|
||||
self.feedConfig = None
|
||||
@ -246,7 +284,7 @@ class Feeding(QObject):
|
||||
self.reset_status = ResetStatus.RNone
|
||||
self.reversed_positions = []
|
||||
self.current_position = None
|
||||
self.index=1
|
||||
self.index = 1
|
||||
self.pos_index = -1
|
||||
self.pos_near_index = -1
|
||||
self.catch = Catch(self.robotClient)
|
||||
@ -257,7 +295,7 @@ class Feeding(QObject):
|
||||
self.onekey = False
|
||||
self.debug_run_count = 0 # 初始化计数器
|
||||
self.mid_take_count = 0
|
||||
#传感器判断抓包参数
|
||||
# 传感器判断抓包参数
|
||||
self.sensor2_ready = False # 传感器2是否检测到料包
|
||||
self.motor_stopped_by_sensor2 = False # 是否由传感器2触发停止电机
|
||||
self.sensor_thread = None
|
||||
@ -272,9 +310,11 @@ class Feeding(QObject):
|
||||
pass
|
||||
|
||||
def close_feed(self):
|
||||
self.need_origin_signal.disconnect()
|
||||
self.relay_controller._running = False
|
||||
self.is_detected = False
|
||||
self.detect_thread.join()
|
||||
self.detect.detection.release()
|
||||
# self.detect.detection.release()
|
||||
|
||||
def init_detection_image(self):
|
||||
detection_image = cv2.imread(Constant.feed_sign_path)
|
||||
@ -290,12 +330,14 @@ class Feeding(QObject):
|
||||
# 获取事件坐标
|
||||
real_position = Real_Position()
|
||||
self.detect.position_index = 0
|
||||
real_position.init_position(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)
|
||||
real_position.init_position(
|
||||
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,
|
||||
)
|
||||
# real_position.init_position(0,
|
||||
# 0,
|
||||
# 0,
|
||||
@ -312,7 +354,12 @@ class Feeding(QObject):
|
||||
if self.feedConfig == None:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
|
||||
if self.feedConfig !=None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(real_position,is_action=True):
|
||||
if (
|
||||
self.feedConfig != None
|
||||
and self.feedConfig.num == 0
|
||||
and self.is_reverse
|
||||
and self.robotClient.origin_position.compare(real_position, is_action=True)
|
||||
):
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.is_reverse = False
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
|
||||
@ -338,15 +385,24 @@ class Feeding(QObject):
|
||||
else:
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
|
||||
if self.feedConfig.feedLine.start_to_take[0].get_position().compare(real_position):
|
||||
if (
|
||||
self.feedConfig.feedLine.start_to_take[0]
|
||||
.get_position()
|
||||
.compare(real_position)
|
||||
):
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FStart:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_start)
|
||||
self.relay_controller.open(conveyor2=True)#开电机
|
||||
#self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢
|
||||
#self.sensor2_thread.start()
|
||||
if not self.robotClient.origin_position.compare(real_position,is_action=True) and not self.is_reverse:
|
||||
self.relay_controller.open(conveyor2=True) # 开电机
|
||||
# self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢
|
||||
# self.sensor2_thread.start()
|
||||
if (
|
||||
not self.robotClient.origin_position.compare(
|
||||
real_position, is_action=True
|
||||
)
|
||||
and not self.is_reverse
|
||||
):
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_start_error)
|
||||
self.need_origin_signal.emit(Constant.str_feed_start_error)
|
||||
@ -373,19 +429,23 @@ class Feeding(QObject):
|
||||
if Constant.Debug1:
|
||||
self.log_signal.emit(
|
||||
logging.INFO,
|
||||
f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次"
|
||||
f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次",
|
||||
)
|
||||
#这里他写的中间点过完马上进行抓料，和整个流程不符合我就注释掉了，因为实际测试的时候还没到FTake的点他就进行了继电器关闭夹爪的抓取
|
||||
#if self.feedStatus == FeedStatus.FTake:
|
||||
#self.catch.catch_status = CatchStatus.CTake
|
||||
# 这里他写的中间点过完马上进行抓料，和整个流程不符合我就注释掉了，因为实际测试的时候还没到FTake的点他就进行了继电器关闭夹爪的抓取
|
||||
# if self.feedStatus == FeedStatus.FTake:
|
||||
# self.catch.catch_status = CatchStatus.CTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
if self.feedConfig.num == 0:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_finish)
|
||||
self.is_reverse = True
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
self.feedConfig.feedLine.start2take_pos_index = len(self.feedConfig.feedLine.start_to_take) - 2
|
||||
self.feedConfig.feedLine.origin2start_pos_index = len(self.feedConfig.feedLine.origin_to_start) - 2
|
||||
self.feedConfig.feedLine.start2take_pos_index = (
|
||||
len(self.feedConfig.feedLine.start_to_take) - 2
|
||||
)
|
||||
self.feedConfig.feedLine.origin2start_pos_index = (
|
||||
len(self.feedConfig.feedLine.origin_to_start) - 2
|
||||
)
|
||||
self.next_position(self.is_reverse)
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
|
||||
self.init_detection_image()
|
||||
@ -395,15 +455,19 @@ class Feeding(QObject):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto)
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Feed
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto_success)
|
||||
self.feedConfig.feedLine.set_take_position(self.detect.detect_position, 0)
|
||||
#self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置
|
||||
'''real_position'''
|
||||
self.feedConfig.feedLine.set_take_position(
|
||||
self.detect.detect_position, 0
|
||||
)
|
||||
# self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置
|
||||
"""real_position"""
|
||||
# 一直等待传感器2信号，永不超时
|
||||
while True:
|
||||
sensors = self.relay_controller.get_all_device_status('sensors')
|
||||
sensors = self.relay_controller.get_all_device_status("sensors")
|
||||
sensor2_value = sensors.get(self.relay_controller.SENSOR2, False)
|
||||
if sensor2_value:
|
||||
self.log_signal.emit(logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取")
|
||||
self.log_signal.emit(
|
||||
logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取"
|
||||
)
|
||||
break # ✅ 条件满足，跳出循环，继续执行下面的代码
|
||||
else:
|
||||
self.log_signal.emit(logging.INFO, "⏳ 等待传感器2料包信号...")
|
||||
@ -411,7 +475,7 @@ class Feeding(QObject):
|
||||
self.next_position()
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing2)
|
||||
|
||||
#self.feedStatus = FeedStatus.FTake
|
||||
# self.feedStatus = FeedStatus.FTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FTake:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_take)
|
||||
@ -424,31 +488,33 @@ class Feeding(QObject):
|
||||
return
|
||||
self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位")
|
||||
# 执行抓取动作
|
||||
#self.catch.catch_status = CatchStatus.CTake
|
||||
#if self.catch.catch_status == CatchStatus.CNone:
|
||||
#self.catch.catch_status = CatchStatus.CTake
|
||||
#if self.catch.catch_status == CatchStatus.CTake:
|
||||
#self.log_signal.emit(logging.INFO, "正在执行抓料动作...")
|
||||
#self.catch.catch_status = CatchStatus.COk
|
||||
#if self.catch.catch_status == CatchStatus.COk:
|
||||
#self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
|
||||
#self.catch.catch_status = CatchStatus.CNone
|
||||
# 移动到下一个抓取点
|
||||
# 更新丢包点: 如果需要根据放置情况调整下次抓取
|
||||
# self.catch.catch_status = CatchStatus.CTake
|
||||
# if self.catch.catch_status == CatchStatus.CNone:
|
||||
# self.catch.catch_status = CatchStatus.CTake
|
||||
# if self.catch.catch_status == CatchStatus.CTake:
|
||||
# self.log_signal.emit(logging.INFO, "正在执行抓料动作...")
|
||||
# self.catch.catch_status = CatchStatus.COk
|
||||
# if self.catch.catch_status == CatchStatus.COk:
|
||||
# self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
|
||||
# self.catch.catch_status = CatchStatus.CNone
|
||||
# 移动到下一个抓取点
|
||||
# 更新丢包点: 如果需要根据放置情况调整下次抓取
|
||||
next_drop_pos = self.drop_manager.get_next_drop_position(lineid=1)
|
||||
print("next_drop_pos--------",next_drop_pos)
|
||||
print("next_drop_pos--------", next_drop_pos)
|
||||
if next_drop_pos:
|
||||
self.feedConfig.feedLine.set_drop_position(next_drop_pos)
|
||||
self.log_signal.emit(logging.INFO, f"已设置放置点: X={next_drop_pos.X:.2f}")
|
||||
self.log_signal.emit(
|
||||
logging.INFO, f"已设置放置点: X={next_drop_pos.X:.2f}"
|
||||
)
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, "获取放置点失败")
|
||||
return
|
||||
#self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
|
||||
# self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
|
||||
self.relay_controller.open(clamp=True)
|
||||
self.next_position()
|
||||
#return
|
||||
#else:
|
||||
#self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
# return
|
||||
# else:
|
||||
# self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken1:
|
||||
|
||||
@ -456,7 +522,6 @@ class Feeding(QObject):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken2:
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
@ -464,7 +529,11 @@ class Feeding(QObject):
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FShake:
|
||||
if self.get_current_position().get_position().compare(real_position,is_action=True):
|
||||
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:
|
||||
@ -477,21 +546,25 @@ class Feeding(QObject):
|
||||
# else:
|
||||
self.catch.shake_continue.SetReset()
|
||||
self.next_position()
|
||||
if self.feedStatus!=FeedStatus.FShake:
|
||||
if self.feedStatus != FeedStatus.FShake:
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
return
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropBag:
|
||||
#"""*** 处理投料点 (FDropBag) 的核心逻辑 ***"""
|
||||
# 1. 确认机械臂是否已精确到达当前目标投料点
|
||||
# get_current_position() 会根据 self.feed_Mid_Status (应为 FMid_Feed)
|
||||
# 调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取
|
||||
# 由 feeding2end_pos_index 指向的点。
|
||||
if self.get_current_position().get_position().compare(real_position, is_action=True):
|
||||
# """*** 处理投料点 (FDropBag) 的核心逻辑 ***"""
|
||||
# 1. 确认机械臂是否已精确到达当前目标投料点
|
||||
# get_current_position() 会根据 self.feed_Mid_Status (应为 FMid_Feed)
|
||||
# 调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取
|
||||
# 由 feeding2end_pos_index 指向的点。
|
||||
if (
|
||||
self.get_current_position()
|
||||
.get_position()
|
||||
.compare(real_position, is_action=True)
|
||||
):
|
||||
# 2. 记录日志：已到达投料点
|
||||
#if not self.sensor2_ready:
|
||||
#self.log_signal.emit(logging.INFO, "抓取完成，重新启动 conveyor2")
|
||||
#self.relay_controller.open(conveyor2=True)
|
||||
# if not self.sensor2_ready:
|
||||
# self.log_signal.emit(logging.INFO, "抓取完成，重新启动 conveyor2")
|
||||
# self.relay_controller.open(conveyor2=True)
|
||||
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_drop)
|
||||
# 3. 与 Catch 模块进行状态交互来驱动投料动作
|
||||
@ -519,7 +592,10 @@ class Feeding(QObject):
|
||||
# self.feedConfig.feedLine.set_take_position(...)
|
||||
# 4. 更新业务逻辑：减少剩余袋数
|
||||
self.feedConfig.num = self.feedConfig.num - 1
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
|
||||
self.log_signal.emit(
|
||||
logging.INFO,
|
||||
f"{Constant.str_feed_feed_num}{self.feedConfig.num}",
|
||||
)
|
||||
# 5. *** 关键步骤 ***: 移动到路径中的下一个点
|
||||
# next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
|
||||
# 调用 next_Feed()。
|
||||
@ -527,17 +603,20 @@ class Feeding(QObject):
|
||||
|
||||
def run_reset(self):
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(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)
|
||||
real_position.init_position(
|
||||
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,
|
||||
)
|
||||
if self.reset_status == ResetStatus.RNone:
|
||||
return
|
||||
|
||||
if self.reset_status == ResetStatus.RStart:
|
||||
if self.feedConfig == None: return
|
||||
if self.feedConfig == None:
|
||||
return
|
||||
# for index in range(len(self.feedConfig.feedLine.positions)):
|
||||
# if self.feedConfig.feedLine.positions[index].status == 2:
|
||||
# start_index = index
|
||||
@ -545,23 +624,36 @@ class Feeding(QObject):
|
||||
self.pos_near_index = -1
|
||||
self.reversed_positions = []
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if pos_model.get_position().compare(real_position,is_action=True):
|
||||
if pos_model.get_position().compare(real_position, is_action=True):
|
||||
self.pos_index = index
|
||||
break
|
||||
|
||||
if self.pos_index == -1:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
|
||||
self.log_signal.emit(
|
||||
logging.ERROR, Constant.str_feed_return_original_position_fail
|
||||
)
|
||||
min_distance = 99999999
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if get_distance(pos_model.get_position(), real_position) < min_distance:
|
||||
min_distance = get_distance(pos_model.get_position(), real_position)
|
||||
for index, pos_model in enumerate(
|
||||
self.feedConfig.feedLine.feed_positions
|
||||
):
|
||||
if (
|
||||
get_distance(pos_model.get_position(), real_position)
|
||||
< min_distance
|
||||
):
|
||||
min_distance = get_distance(
|
||||
pos_model.get_position(), real_position
|
||||
)
|
||||
self.pos_near_index = index
|
||||
if self.pos_near_index != -1:
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_near_index + 1]
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[
|
||||
: self.pos_near_index + 1
|
||||
]
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index+1]
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[
|
||||
: self.pos_index + 1
|
||||
]
|
||||
self.reversed_positions = list(reversed(self.reversed_positions))
|
||||
self.reverse_index = 0
|
||||
self.send_emergency_sound()
|
||||
@ -570,21 +662,30 @@ class Feeding(QObject):
|
||||
self.reset_status = ResetStatus.RRunging
|
||||
|
||||
if self.reset_status == ResetStatus.RRunging:
|
||||
if not real_position.compare(self.current_position.get_position(),is_action=True):
|
||||
if not real_position.compare(
|
||||
self.current_position.get_position(), is_action=True
|
||||
):
|
||||
return
|
||||
pos_model = self.reversed_positions[self.reverse_index]
|
||||
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
|
||||
pos_model = self.reversed_positions[self.reverse_index + 1]
|
||||
self.reverse_index = self.reverse_index+1
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
|
||||
if pos_model.lineType == LineType.CureMid.value:
|
||||
pos_model1 = self.reversed_positions[self.reverse_index + 1]
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.sendTargPosition(
|
||||
real_position=pos_model.get_position(),
|
||||
move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(),
|
||||
speed=self.robotClient.reset_speed,
|
||||
)
|
||||
self.current_position = pos_model1
|
||||
self.reverse_index = self.reverse_index + 2
|
||||
else:
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.sendTargPosition(
|
||||
real_position=pos_model.get_position(),
|
||||
speed=self.robotClient.reset_speed,
|
||||
)
|
||||
self.current_position = pos_model
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
|
||||
@ -598,13 +699,16 @@ class Feeding(QObject):
|
||||
|
||||
self.run_reverse = True
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(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)
|
||||
if self.feedConfig == None: return
|
||||
real_position.init_position(
|
||||
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,
|
||||
)
|
||||
if self.feedConfig == None:
|
||||
return
|
||||
start_index = -1
|
||||
# for index in range(len(self.feedConfig.feedLine.positions)):
|
||||
# if self.feedConfig.feedLine.positions[index].status == 2:
|
||||
@ -618,14 +722,18 @@ class Feeding(QObject):
|
||||
break
|
||||
|
||||
if pos_index == -1:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
|
||||
self.log_signal.emit(
|
||||
logging.ERROR, Constant.str_feed_return_original_position_fail
|
||||
)
|
||||
min_distance = 99999999
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if get_distance(pos_model.get_position(), real_position)<min_distance:
|
||||
if get_distance(pos_model.get_position(), real_position) < min_distance:
|
||||
min_distance = get_distance(pos_model.get_position(), real_position)
|
||||
pos_near_index = index
|
||||
if pos_near_index != -1:
|
||||
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_near_index+1]
|
||||
reversed_positions = self.feedConfig.feedLine.feed_positions[
|
||||
: pos_near_index + 1
|
||||
]
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
@ -635,10 +743,12 @@ class Feeding(QObject):
|
||||
self.send_emergency_sound()
|
||||
current_position = PositionModel()
|
||||
current_position.init_position(real_position)
|
||||
while self.run_reverse and self.reverse_index!=len(reversed_positions):
|
||||
if self.reverse_index!=0 and not real_position.compare(current_position.get_position()):
|
||||
while self.run_reverse and self.reverse_index != len(reversed_positions):
|
||||
if self.reverse_index != 0 and not real_position.compare(
|
||||
current_position.get_position()
|
||||
):
|
||||
continue
|
||||
#todo 缺少比对
|
||||
# todo 缺少比对
|
||||
pos_model = reversed_positions[self.reverse_index]
|
||||
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
|
||||
|
||||
@ -647,16 +757,24 @@ class Feeding(QObject):
|
||||
|
||||
if pos_model.lineType == LineType.CureMid.value:
|
||||
pos_model1 = reversed_positions[self.reverse_index + 1]
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(),speed= self.robotClient.reset_speed)
|
||||
self.sendTargPosition(
|
||||
real_position=pos_model.get_position(),
|
||||
move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(),
|
||||
speed=self.robotClient.reset_speed,
|
||||
)
|
||||
current_position = pos_model1
|
||||
self.reverse_index = self.reverse_index+2
|
||||
self.reverse_index = self.reverse_index + 2
|
||||
else:
|
||||
self.sendTargPosition(real_position=pos_model.get_position(),speed=self.robotClient.reset_speed)
|
||||
self.sendTargPosition(
|
||||
real_position=pos_model.get_position(),
|
||||
speed=self.robotClient.reset_speed,
|
||||
)
|
||||
current_position = pos_model
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
self.send_emergency_stop()
|
||||
return True
|
||||
|
||||
def send_emergency_sound(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 1)
|
||||
|
||||
@ -670,17 +788,30 @@ class Feeding(QObject):
|
||||
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.dsID = "HCRemoteCommand"
|
||||
IO_command.instructions.append(io_instruction)
|
||||
self.robotClient.add_sendQuene(IO_command.toString())
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
|
||||
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):
|
||||
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):
|
||||
def sendTargPosition(
|
||||
self,
|
||||
real_position,
|
||||
move_type: MoveType = MoveType.WORLD,
|
||||
speed=5,
|
||||
real_position1=None,
|
||||
):
|
||||
position_instruction = Instruction()
|
||||
position_instruction.speed = speed
|
||||
position_instruction.m0 = real_position.X
|
||||
@ -703,24 +834,26 @@ class Feeding(QObject):
|
||||
instruction_command.instructions.append(position_instruction)
|
||||
request_command = instruction_command.toString()
|
||||
|
||||
log_str = f'移动到位置:{ "姿势直线" if move_type==MoveType.WORLD else "姿势曲线"}：' \
|
||||
f'X:{position_instruction.m0}-' \
|
||||
f'Y:{position_instruction.m1}-' \
|
||||
f'Z:{position_instruction.m2}-' \
|
||||
f'U:{position_instruction.m3}-' \
|
||||
f'V:{position_instruction.m4}-' \
|
||||
f'W:{position_instruction.m5}'
|
||||
|
||||
log_str = (
|
||||
f'移动到位置:{ "姿势直线" if move_type==MoveType.WORLD else "姿势曲线"}：'
|
||||
f"X:{position_instruction.m0}-"
|
||||
f"Y:{position_instruction.m1}-"
|
||||
f"Z:{position_instruction.m2}-"
|
||||
f"U:{position_instruction.m3}-"
|
||||
f"V:{position_instruction.m4}-"
|
||||
f"W:{position_instruction.m5}"
|
||||
)
|
||||
|
||||
try:
|
||||
print('fuck1',log_str)
|
||||
print("fuck1", log_str)
|
||||
self.log_signal.emit(logging.INFO, log_str)
|
||||
print('fuck2',log_str)
|
||||
print("fuck2", log_str)
|
||||
except:
|
||||
return
|
||||
print(request_command)
|
||||
self.robotClient.add_sendQuene(request_command)
|
||||
pass
|
||||
|
||||
# def get_take_position(self):
|
||||
# if Constant.Debug:
|
||||
# return self.robotClient.status_model.getRealPosition()
|
||||
@ -733,59 +866,117 @@ class Feeding(QObject):
|
||||
# position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
|
||||
# position.Z = position.Z+200
|
||||
# return position
|
||||
def next_start(self,reverse=False):
|
||||
def next_start(self, reverse=False):
|
||||
start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.feedStatus = (
|
||||
FeedStatus(start_pos.status)
|
||||
if self.feedStatus != FeedStatus.FNone
|
||||
else FeedStatus.FNone
|
||||
)
|
||||
if start_pos.lineType == LineType.CureMid.value:
|
||||
start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
self.feedStatus = FeedStatus(start_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed, move_type=MoveType.Cure, real_position1=start_pos1.get_position())
|
||||
self.feedStatus = (
|
||||
FeedStatus(start_pos1.status)
|
||||
if self.feedStatus != FeedStatus.FNone
|
||||
else FeedStatus.FNone
|
||||
)
|
||||
self.sendTargPosition(
|
||||
real_position=start_pos.get_position(),
|
||||
speed=self.robotClient.reset_speed,
|
||||
move_type=MoveType.Cure,
|
||||
real_position1=start_pos1.get_position(),
|
||||
)
|
||||
elif start_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(start_pos.get_position(),self.robotClient.max_angle_interval):
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(
|
||||
start_pos.get_position(), self.robotClient.max_angle_interval
|
||||
):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed,move_type=MoveType.AXIS)
|
||||
self.sendTargPosition(
|
||||
real_position=start_pos.get_position(),
|
||||
speed=self.robotClient.reset_speed,
|
||||
move_type=MoveType.AXIS,
|
||||
)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed)
|
||||
self.sendTargPosition(
|
||||
real_position=start_pos.get_position(),
|
||||
speed=self.robotClient.reset_speed,
|
||||
)
|
||||
pass
|
||||
|
||||
def next_take(self,reverse=False):
|
||||
def next_take(self, reverse=False):
|
||||
|
||||
take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
self.feedStatus = FeedStatus(take_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.feedStatus = (
|
||||
FeedStatus(take_pos.status)
|
||||
if self.feedStatus != FeedStatus.FNone
|
||||
else FeedStatus.FNone
|
||||
)
|
||||
if take_pos.lineType == LineType.CureMid.value:
|
||||
take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
self.feedStatus = FeedStatus(take_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure, real_position1=take_pos1.get_position(),speed=self.robotClient.feed_speed)
|
||||
self.feedStatus = (
|
||||
FeedStatus(take_pos1.status)
|
||||
if self.feedStatus != FeedStatus.FNone
|
||||
else FeedStatus.FNone
|
||||
)
|
||||
self.sendTargPosition(
|
||||
real_position=take_pos.get_position(),
|
||||
move_type=MoveType.Cure,
|
||||
real_position1=take_pos1.get_position(),
|
||||
speed=self.robotClient.feed_speed,
|
||||
)
|
||||
elif take_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(take_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(
|
||||
take_pos.get_position(), self.robotClient.max_angle_interval
|
||||
):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
self.sendTargPosition(
|
||||
real_position=take_pos.get_position(),
|
||||
speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS,
|
||||
)
|
||||
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
self.sendTargPosition(
|
||||
real_position=take_pos.get_position(), speed=self.robotClient.feed_speed
|
||||
)
|
||||
pass
|
||||
|
||||
def next_Feed(self,reverse=False):
|
||||
def next_Feed(self, reverse=False):
|
||||
feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
self.feedStatus = FeedStatus(feed_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.feedStatus = (
|
||||
FeedStatus(feed_pos.status)
|
||||
if self.feedStatus != FeedStatus.FNone
|
||||
else FeedStatus.FNone
|
||||
)
|
||||
if feed_pos.lineType == LineType.CureMid.value:
|
||||
feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
self.feedStatus = FeedStatus(feed_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure, real_position1=feed_pos1.get_position(),speed=self.robotClient.feed_speed)
|
||||
self.feedStatus = (
|
||||
FeedStatus(feed_pos1.status)
|
||||
if self.feedStatus != FeedStatus.FNone
|
||||
else FeedStatus.FNone
|
||||
)
|
||||
self.sendTargPosition(
|
||||
real_position=feed_pos.get_position(),
|
||||
move_type=MoveType.Cure,
|
||||
real_position1=feed_pos1.get_position(),
|
||||
speed=self.robotClient.feed_speed,
|
||||
)
|
||||
elif feed_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(feed_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(
|
||||
feed_pos.get_position(), self.robotClient.max_angle_interval
|
||||
):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
self.sendTargPosition(
|
||||
real_position=feed_pos.get_position(),
|
||||
speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS,
|
||||
)
|
||||
|
||||
else:
|
||||
# if not reverse and self.feedStatus == FeedStatus.FShake:
|
||||
@ -797,29 +988,31 @@ class Feeding(QObject):
|
||||
# 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
|
||||
)
|
||||
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
|
||||
def get_current_position(self,is_reverse=False):
|
||||
def get_current_position(self, is_reverse=False):
|
||||
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
return self.feedConfig.feedLine.get_current_start_position(self.is_reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
return self.feedConfig.feedLine.get_current_take_position(self.is_reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
return self.feedConfig.feedLine.get_current_feed_position(self.is_reverse)
|
||||
def next_position(self,reverse=False):
|
||||
|
||||
def next_position(self, reverse=False):
|
||||
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
self.next_start(reverse)
|
||||
if Constant.Debug1:
|
||||
print('next_start')
|
||||
print("next_start")
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
self.next_take(reverse)
|
||||
if Constant.Debug1:
|
||||
print('next_take')
|
||||
print("next_take")
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
self.next_Feed(reverse)
|
||||
if Constant.Debug1:
|
||||
print('next_feed')
|
||||
print("next_feed")
|
||||
|
||||
def safe_check_columns(self):
|
||||
return True
|
||||
@ -828,4 +1021,3 @@ class Feeding(QObject):
|
||||
def safe_check_person(self):
|
||||
return True
|
||||
pass
|
||||
|
||||
|
||||
Reference in New Issue
Block a user