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-12-03 23:25:17 +08:00
parent 157e7edce5
commit aa93685862
8 changed files with 728 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
COM/COM_Robot.py
View File

@ -26,7 +26,7 @@ class RobotClient(TCPClient):
self.time_delay_take= time_delay_take
self.time_delay_put = time_delay_put
self.time_delay_shake = time_delay_shake
self.type_detection = DetectType.EyeOnHand
self.type_detection = DetectType.EyeOutHand
def add_sendQuene(self,command): #后面 命令分等级，紧急命令直接执行
self.command_quene.put(command)
@ -95,6 +95,7 @@ class RobotClient(TCPClient):
raise
def get_origin_position(self):
return self.status_model.getRealPosition()
pass

344
CU/Feeding.py
View File

@ -8,41 +8,131 @@ import Constant
import Expection
from Model.Position import Real_Position, Detection_Position
from enum import Enum, IntEnum
from COM.COM_Robot import RobotClient
from COM.COM_Robot import RobotClient, DetectType
from Model.RobotModel import CMDInstructRequest, MoveType
from Util.util_time import CRisOrFall
from Vision.camera_coordinate_dete import Detection
from Util.util_log import log
from Model.RobotModel import Instruction
class FeedStatus(IntEnum):
FNone = 0
FStart = 1
FCheck = 2
FSafeP = 3
FMid = 3
FPhoto = 4
FTake = 5
FSafeF = 6
FFeedP = 7
FBroken = 8
FCut = 9
FShake = 10
FDropBag = 11
FFinished = 12
FBroken1 = 6
FBroken2 =7
FShake = 8
FDropBag = 9
FFinished = 10
FReverse = 11
FStartReverse = 12
class LineType(Enum):
Straight = 0
CureMid = 2
CureEnd = 3
class FeedMidStatus(Enum):
FMid_Start = 1
FMid_Take = 2
FMid_Feed= 3
class FeedPosition:
def __init__(self,status:FeedStatus,position:Real_Position):
self.status = status
self.position = position
class FeedLine:
def __init__(self,id,name,safe_position:Real_Position,broken1_position:Real_Position,broken2_position:Real_Position,shake_position:Real_Position,drop_bag_position:Real_Position):
self.safe_position = safe_position
self.broken1_position = broken1_position
self.broken2_position = broken2_position
self.shake_position = shake_position
self.drop_bag_position = drop_bag_position
self.take_position = None
def __init__(self, id, name, feed_positions:list):
self.feed_positions = feed_positions
self.feeding2end_pos_index = 0
self.origin2start_pos_index = 0
self.start2take_pos_index = 0
self.name = name
self.id = id
self.get_position_list()
def get_current_feed_position(self):
pos = self.feeding_to_end[self.feeding2end_pos_index]
return pos
def get_current_take_position(self):
pos = self.start_to_take[self.start2take_pos_index]
return pos
def get_current_start_position(self):
pos = self.origin_to_start[self.origin2start_pos_index]
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
if self.feeding2end_pos_index < 0:
self.feeding2end_pos_index = len(self.feeding_to_end) - 1
else:
self.feeding2end_pos_index += 1
if self.feeding2end_pos_index >= len(self.feeding_to_end):
self.feeding2end_pos_index = 0
return pos
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
if self.origin2start_pos_index < 0:
self.origin2start_pos_index = len(self.origin_to_start) - 1
else:
self.origin2start_pos_index += 1
if self.origin2start_pos_index >= len(self.origin_to_start):
self.origin2start_pos_index = 0
return pos
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
if self.start2take_pos_index < 0:
self.start2take_pos_index = len(self.start_to_take) - 1
else:
self.start2take_pos_index += 1
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:
return self.feeding_to_end[i]
def set_take_position(self,position:Real_Position):
for i in range(len(self.feeding_to_end)):
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
self.feeding_to_end[i].set_position(position)
def get_position_list(self):
index_start = -1
for i in range(len(self.feed_positions)):
if self.feed_positions[i].status == FeedStatus.FCheck.value:
index_start = i
break
for i in range(len(self.feed_positions)):
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.feeding_to_end = self.feed_positions[index_take:]
class FeedingConfig:
@ -70,11 +160,13 @@ class Feeding :
self.init_detection_image()
self.pause = False
self.cRis_photo = CRisOrFall()
self.feed_Mid_Status = FeedMidStatus.FMid_Start
self.is_reverse = False
pass
def init_detection_image(self):
self.detection_image = cv2.imread(Constant.feed_sign_path)
def run(self):
# 获取事件坐标
real_position = Real_Position()
@ -91,18 +183,13 @@ class Feeding :
# 0,
# 0);
if self.feedConfig == None:
self.feedStatus = FeedStatus.FNone
elif self.feedConfig.num == 0:
elif self.feedConfig.num == 0 and self.feedStatus!=FeedStatus.FReverse:
self.feedStatus = FeedStatus.FNone
if self.feedStatus == FeedStatus.FNone or self.pause:
return
elif self.feedStatus == FeedStatus.FStart:
log.log_message(logging.INFO, Constant.str_feed_start)
self.feedStatus = FeedStatus.FCheck if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
pass
elif self.feedStatus == FeedStatus.FCheck:
log.log_message(logging.INFO, Constant.str_feed_check)
# 1, 检查是否是三列
@ -110,21 +197,57 @@ class Feeding :
# if self.safe_check_columns() and self.safe_check_person():
# pass
# else:
if self.feedConfig.num != 0:
self.feedStatus = FeedStatus.FSafeP if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
# if self.feedConfig.num != 0:
# self.next_target()
# if == 原点 继续判断
# else:
# QMessageBox.information(None, "提示", Constant.str_feed_safe_error_msgbox)
if self.is_reverse:
self.feed_Mid_Status = FeedMidStatus.FMid_Start
else:
self.feed_Mid_Status = FeedMidStatus.FMid_Take
self.next_position(self.is_reverse)
elif self.feedStatus == FeedStatus.FStart:
log.log_message(logging.INFO, Constant.str_feed_start)
if not self.robotClient.get_origin_position().compare(real_position) and not self.is_reverse:
QMessageBox.information(None, "提示", Constant.str_feed_start_error)
self.feedStatus = FeedStatus.FNone
return
if self.is_reverse and self.robotClient.get_origin_position().compare(real_position):
self.feedStatus = FeedStatus.FNone
self.is_reverse = False
return
# TODO 获取目标位置
self.feedConfig.feedLine.set_take_position(real_position)
# 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.feed_Mid_Status = FeedMidStatus.FMid_Start
self.next_position(self.is_reverse)
elif self.feedStatus == FeedStatus.FMid:
log.log_message(logging.INFO, Constant.str_feed_mid)
feed_pos = self.get_current_position()
if feed_pos.get_position().compare(real_position):
self.next_position(self.is_reverse)
elif self.feedStatus==FeedStatus.FSafeP:
log.log_message(logging.INFO,Constant.str_feed_safe)
if self.feedConfig.feedLine.safe_position.compare(real_position):
self.error_photo_count=0
self.feedStatus = FeedStatus.FPhoto if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
#self.sendTargPosition(self.feedConfig.feedLine.photo_position)
#判断是哪一个问题
elif self.feedStatus == FeedStatus.FPhoto:
log.log_message(logging.INFO, Constant.str_feed_photo)
if self.feedConfig.num == 0:
log.log_message(logging.INFO, Constant.str_feed_finish)
self.is_reverse = True
self.FeedMid_Status = FeedMidStatus.FMid_Take
self.next_position(self.is_reverse)
return
if self.robotClient.type_detection == DetectType.EyeOutHand:
self.feed_Mid_Status = FeedMidStatus.FMid_Feed
self.next_position()
return
detect_pos_list = []
if not Constant.Debug:
try:
@ -133,6 +256,8 @@ class Feeding :
QMessageBox.information(None, "提示", Constant.str_feed_photo_error_msgbox)
self.error_photo_count = 0
log.log_message(logging.INFO, Constant.str_feed_photo_confirm)
# TODO 返回初始状态
for pos in self.feedConfig.photo_locs:
self.sendTargPosition(pos)
while not pos.compare(real_position): # 可以优化 TODO
@ -147,7 +272,7 @@ class Feeding :
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, #TODO 这个10 需要确定
self.robotClient.status_model.world_2,
self.robotClient.status_model.world_3,
self.robotClient.status_model.world_4,
self.robotClient.status_model.world_5)
@ -163,97 +288,85 @@ class Feeding :
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 保留全部的开关
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]
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]
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]
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]
take_position = detect_pos_list[max_z_index]
log.log_message(logging.INFO, Constant.str_feed_takePhoto_line)
self.feedStatus = FeedStatus.FTake if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.take_position)
self.feedConfig.feedLine.set_take_position(take_position) ##TODO 检查有没有异常
self.next_position()
except:
log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail)
self.error_photo_count += 1
else:
self.feedConfig.feedLine.take_position = self.feedConfig.feedLine.safe_position
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.feedStatus = FeedStatus.FTake if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
self.feedConfig.feedLine.set_take_position(real_position) ##TODO 检查有没有异常
self.next_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):
if self.feedConfig.feedLine.get_take_position().get_position() != None:
if self.feedConfig.feedLine.get_take_position().get_position().compare(real_position):
# 打开吸嘴并返回
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 检测是否通 不然报警
self.feedConfig.feedLine.set_take_position(None)
time.sleep(self.robotClient.time_delay_take)
self.feedStatus = FeedStatus.FSafeF if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
log.log_message(logging.INFO, Constant.str_feed_take_success)
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.next_position()
pass
elif self.feedStatus == FeedStatus.FSafeF:
log.log_message(logging.INFO, Constant.str_feed_safe)
if self.feedConfig.feedLine.safe_position.compare(real_position):
self.feedStatus = FeedStatus.FBroken if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.broken1_position)
pass #吸嘴开始
# elif self.feedStatus==FeedStatus.FFeedP:
# 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.broken1_position)
# pass #破袋
elif self.feedStatus==FeedStatus.FBroken:
elif self.feedStatus == FeedStatus.FBroken1:
log.log_message(logging.INFO, Constant.str_feed_broken)
if self.get_current_position().get_position().compare(real_position):
self.next_position()
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):
self.sendTargPosition(self.feedConfig.feedLine.shake_position)
if self.feedConfig.feedLine.shake_position.compare(real_position): # 延迟判断，如果最后点位延迟1s，则认为阶段完成
elif self.feedStatus == FeedStatus.FBroken2:
log.log_message(logging.INFO, Constant.str_feed_broken)
if self.get_current_position().get_position().compare(real_position):
self.next_position()
elif self.feedStatus == FeedStatus.FShake:
log.log_message(logging.INFO, Constant.str_feed_shake)
if self.get_current_position().get_position().compare(real_position):
# TODO 震动方案
time.sleep(self.robotClient.time_delay_shake)
self.feedStatus = FeedStatus.FDropBag if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.next_position()
elif self.feedStatus == FeedStatus.FDropBag:
log.log_message(logging.INFO, Constant.str_feed_drop)
if self.feedConfig.feedLine.safe_position.compare(real_position):
self.sendTargPosition(self.feedConfig.feedLine.drop_bag_position)
pass
if self.feedConfig.feedLine.drop_bag_position.compare(real_position):
if self.get_current_position().get_position().compare(real_position):
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(self.robotClient.time_delay_put)
# TODO 获取目标位置
# 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(real_position)
self.feedConfig.num = self.feedConfig.num - 1
log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
if self.feedConfig.num == 0: # 投料次数为0，直接返回
log.log_message(logging.INFO, Constant.str_feed_finish)
self.feedStatus = FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.init_detection_image()
else:
self.feedStatus = FeedStatus.FSafeP if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.next_position()
def send_emergency_sound(self):
@ -275,7 +388,7 @@ class Feeding :
log.log_message(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
pass
def sendTargPosition(self,real_position,move_type:MoveType=MoveType.WORLD ,speed = Constant.speed):
def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=Constant.speed,real_position1=None):
position_instruction = Instruction()
position_instruction.speed = speed
position_instruction.m0 = real_position.X
@ -284,12 +397,19 @@ class Feeding :
position_instruction.m3 = real_position.U
position_instruction.m4 = real_position.V
position_instruction.m5 = real_position.W
position_instruction.action = move_type.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()
log_str = f'移动到位置:{"姿势直线"}：' \
f'X:{position_instruction.m0}-' \
f'Y:{position_instruction.m1}-' \
@ -297,17 +417,59 @@ class Feeding :
f'U:{position_instruction.m3}-' \
f'V:{position_instruction.m4}-' \
f'W:{position_instruction.m5}'
print(log_str)
try:
log.log_message(logging.INFO, log_str)
except:
print("error")
self.robotClient.add_sendQuene(request_command)
pass
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
if start_pos.lineType == LineType.CureMid.value:
start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
self.sendTargPosition(real_position=start_pos.get_position(), move_type=MoveType.Cure, real_position1=start_pos1.get_position())
else:
self.sendTargPosition(real_position=start_pos.get_position())
pass
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
if take_pos.lineType == LineType.CureMid.value:
take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure, real_position1=take_pos1.get_position())
else:
self.sendTargPosition(real_position=take_pos.get_position())
pass
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
if feed_pos.lineType == LineType.CureMid.value:
feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure, real_position1=feed_pos1.get_position())
else:
self.sendTargPosition(real_position=feed_pos.get_position())
def get_current_position(self):
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
return self.feedConfig.feedLine.get_current_start_position()
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
return self.feedConfig.feedLine.get_current_take_position()
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
return self.feedConfig.feedLine.get_current_feed_position()
def next_position(self,reverse=False):
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
self.next_start(reverse)
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
self.next_take(reverse)
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
self.next_Feed(reverse)
def safe_check_columns(self):
return True

29
CU/Feeding_C.py
View File

@ -33,6 +33,13 @@ class FeedStatus(IntEnum):
FReverse = 11
FStartReverse = 12
class LineType(Enum):
Straight = 0
CureStart = 1
CureMid = 2
CureEnd = 3
class FeedMidStatus(Enum):
FMid_Start = 1
FMid_Take = 2
@ -374,7 +381,7 @@ class Feeding:
log.log_message(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
pass
def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=Constant.speed):
def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=Constant.speed,real_position1=None):
position_instruction = Instruction()
position_instruction.speed = speed
position_instruction.m0 = real_position.X
@ -383,7 +390,15 @@ class Feeding:
position_instruction.m3 = real_position.U
position_instruction.m4 = real_position.V
position_instruction.m5 = real_position.W
position_instruction.action = move_type.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()
@ -408,18 +423,30 @@ class Feeding:
def next_start(self,reverse=False):
start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
self.feedStatus = start_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
if start_pos.lineType == LineType.CureMid:
start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
self.sendTargPosition(real_position=start_pos.position, move_type=MoveType.Cure, real_position1=start_pos1.position)
else:
self.sendTargPosition(start_pos.position)
pass
def next_take(self,reverse=False):
take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
self.feedStatus = take_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
if take_pos.lineType == LineType.CureMid:
take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
self.sendTargPosition(real_position=take_pos.position, move_type=MoveType.Cure, real_position1=take_pos1.position)
else:
self.sendTargPosition(take_pos.position)
pass
def next_Feed(self,reverse=False):
feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
self.feedStatus = feed_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
if feed_pos.lineType == LineType.CureMid:
feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
self.sendTargPosition(real_position=feed_pos.position, move_type=MoveType.Cure, real_position1=feed_pos1.position)
else:
self.sendTargPosition(feed_pos.position)
def get_current_position(self):

354
Config/FeedLine.ini
View File

@ -6,75 +6,345 @@ name = 反应釜1
id = 2
name = 反应釜2
[Position1]
id = 1
order = 1
lineid = 1
status = 0
x = 0
y = 0
z = 0
u = 0
v = 0
w = 0
[Position2]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 2
order = 0
lineid = 1
status = 3
linetype = 2
[Position3]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 3
order = 2
lineid = 1
status = 0
x = 0
y = 0
z = 0
u = 0
v = 0
w = 0
status = 2
linetype = 0
[Position4]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 4
order = 4
lineid = 1
status = 3
linetype = 0
[Position5]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 5
order = 6
lineid = 1
status = 3
linetype = 0
[Position1]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 1
order = 8
lineid = 1
status = 3
linetype = 0
[Position7]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 7
order = 1
lineid = 1
status = 3
linetype = 0
[Position8]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 8
order = 3
lineid = 1
status = 3
linetype = 0
[Position9]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 9
order = 5
lineid = 1
status = 3
linetype = 0
[Position10]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 10
order = 7
lineid = 1
status = 3
linetype = 0
[Position11]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 11
order = 9
lineid = 1
status = 4
linetype = 0
[Position12]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 12
order = 10
lineid = 1
status = 5
linetype = 0
[Position13]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 13
order = 11
lineid = 1
status = 3
linetype = 0
[Position14]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 14
order = 12
lineid = 1
status = 3
linetype = 0
[Position15]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 15
order = 13
lineid = 1
status = 6
linetype = 0
[Position16]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 16
order = 14
lineid = 1
status = 7
linetype = 0
[Position17]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 17
order = 15
lineid = 1
status = 8
linetype = 0
[Position18]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 18
order = 16
lineid = 1
status = 3
linetype = 0
[Position19]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 19
order = 17
lineid = 1
status = 9
linetype = 0
[Position20]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 20
order = 18
lineid = 1
status = 3
linetype = 0
[Position21]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 21
order = 19
lineid = 1
status = 3
linetype = 0
[Position6]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
id = 6
order = 20
lineid = 1
status = 3
linetype = 0
[FeedLine3]
id = 3
name = 线3阿斯顿发斯蒂芬
name = 124124
[Position3]
[Position23]
x = 0.0
y = 0.0
z = 0.0
u = 0.0
v = 0.0
w = 0.0
id = 3
order = 5
id = 23
order = 0
lineid = 3
status = 3
status = 1
linetype = 0
[FeedLine4]
id = 4
name = 线4阿斯顿发斯蒂芬
[Position4]
[Position24]
x = 0.0
y = 0.0
z = 0.0
u = 0.0
v = 0.0
w = 0.0
id = 4
order = 5
lineid = 4
status = 3
id = 24
order = 1
lineid = 3
status = 1
linetype = 0
[FeedLine5]
id = 5
name = 线5
[Position5]
[Position25]
x = 0.0
y = 0.0
z = 0.0
u = 0.0
v = 0.0
w = 0.0
id = 5
order = 5
lineid = 5
status = 3
id = 25
order = 2
lineid = 3
status = 1
linetype = 0
[Position26]
x = 0.0
y = 0.0
z = 0.0
u = 0.0
v = 0.0
w = 0.0
id = 26
order = 3
lineid = 3
status = 1
linetype = 0
[Position22]
x = 0.0
y = 0.0
z = 0.0
u = 0.0
v = 0.0
w = 0.0
id = 22
order = 4
lineid = 3
status = 1
linetype = 0

23
Model/FeedModel.py
View File

@ -1,6 +1,7 @@
import configparser
import Constant
from CU.Feeding import LineType
from Model.Position import Real_Position
@ -49,6 +50,7 @@ class PositionModel:
self.order = 0
self.lineId = 0
self.status = 0
self.lineType = 0
self.section = f'Position{index}'
pass
@ -64,12 +66,28 @@ class PositionModel:
self.order = config_reader.getint(self.section, 'order')
self.lineId = config_reader.getint(self.section, 'lineId')
self.status = config_reader.getint(self.section, 'status')
self.lineType = config_reader.getint(self.section, 'lineType')
def get_position(self):
real_pos = Real_Position()
real_pos.init_position(self.X, self.Y, self.Z, self.U, self.V, self.W)
if real_pos.X == -9999:
return None
return real_pos
def set_position(self,real_pos:Real_Position):
if real_pos == None:
self.X = -9999
self.Y = -9999
self.Z = -9999
self.U = -9999
self.V = -9999
self.W = -9999
return
self.X = real_pos.X
self.Y = real_pos.Y
self.Z = real_pos.Z
self.U = real_pos.U
self.V = real_pos.V
self.W = real_pos.W
def save_position_model(self,config_reader):
if not config_reader.has_section(self.section):
config_reader.add_section(self.section)
@ -83,3 +101,4 @@ class PositionModel:
config_reader.set(self.section, 'order', str(self.order))
config_reader.set(self.section, 'lineId', str(self.lineId))
config_reader.set(self.section, 'status', str(self.status))
config_reader.set(self.section, 'lineType', str(self.lineType))

12
Model/RobotModel.py
View File

@ -6,6 +6,7 @@ from Model.Position import Real_Position
class MoveType(Enum):
AXIS = 4
WORLD = 10
Cure = 17
class DATARequest:
def __init__(self):
@ -137,6 +138,12 @@ class Instruction:
self.m3 = 0.0
self.m4 = 0.0
self.m5 = 0.0
self.m0_p = 0.0
self.m1_p = 0.0
self.m2_p = 0.0
self.m3_p = 0.0
self.m4_p = 0.0
self.m5_p = 0.0
self.ckStatus = '0x3F'
self.speed = 10
self.delay = 1.0
@ -153,6 +160,11 @@ class Instruction:
model_str = f'"oneshot":"{self.oneshot}","action":"{self.action}","m0":"{self.m0}","m1":"{self.m1}","m2":"{self.m2}",' \
f'"m3":"{self.m3}","m4":"{self.m4}","m5":"{self.m5}","ckStatus":"{self.ckStatus}","speed":"{self.speed}",' \
f'"delay":"{self.delay}","smooth":"{self.smooth}","tool":"{self.tool}"'
if self.action == 17:
model_str = f'"oneshot":"{self.oneshot}","action":"{self.action}","m0":"{self.m0}","m1":"{self.m1}","m2":"{self.m2}",' \
f'"m3":"{self.m3}","m4":"{self.m4}","m5":"{self.m5}","m0_p":"{self.m0_p}","m1_p":"{self.m1_p}","m2_p":"{self.m2_p}",' \
f'"m3_p":"{self.m3_p}","m4_p":"{self.m4_p}","m5_p":"{self.m5_p}","ckStatus":"{self.ckStatus}","speed":"{self.speed}",' \
f'"delay":"{self.delay}","smooth":"{self.smooth}","tool":"{self.tool}"'
else:
model_str = f'"oneshot":"{self.oneshot}","action":"{200}","type":"{self.type}","io_status":"{self.io_status}"' \
f',"point":"{self.point}","delay":"{self.delay}"'

98
main.py
View File

@ -22,7 +22,7 @@ from Model.FeedModel import LineModel, PositionModel
from Util.util_ini import writeFeedLine_to_ini
import Constant
from CU.Command import FeedCommand
from CU.Feeding import FeedLine, FeedingConfig, Feeding, FeedStatus
from CU.Feeding import FeedLine, FeedingConfig, Feeding, FeedStatus, FeedMidStatus
from Util.util_log import QTextEditLogger
from Util.util_time import CRisOrFall
from Vision.camera_coordinate_dete import Detection
@ -195,22 +195,22 @@ class MainWindow(QMainWindow, Ui_MainWindow):
for row_i in range(self.tableWidget_lines.rowCount()):
if self.tableWidget_lines.item(row_i, 0).text() == section:
line_model = LineModel()
if len(self.get_table_positions()) == 0 and self.feedLine_dict.keys().__contains__(section):
line_model = self.feedLine_dict[section]
line_model.name = self.tableWidget_lines.item(row_i, 1).text()
else:
line_model.section = section
line_model.name = self.tableWidget_lines.item(row_i, 1).text()
line_model.id = int(self.tableWidget_lines.item(row_i, 2).text())
line_model.positions = self.get_table_positions()
if len(self.get_table_positions()) != 0 and line_model.id != self.get_table_positions()[0].id:
return
if len(self.get_table_positions())==0:
return
config_reader = configparser.ConfigParser()
config_reader.read(Constant.feedLine_set_file, encoding='utf-8')
if len(self.get_table_positions()) != 0 and line_model.id != self.get_table_positions()[0].lineId:
return
line_model.save_line_model(config_reader)
config_reader.write(open(Constant.feedLine_set_file, 'w', encoding='utf-8'))
break
self.init_FeedLine()
break
self.init_table_lines_data()
self.table_line_changed = False
self.table_position_changed = False
@ -226,29 +226,36 @@ class MainWindow(QMainWindow, Ui_MainWindow):
position_model.U = float(self.tableWidget_line_positions.item(row_i, 4).text())
position_model.V = float(self.tableWidget_line_positions.item(row_i, 5).text())
position_model.W = float(self.tableWidget_line_positions.item(row_i, 6).text())
widget = self.tableWidget_line_positions.cellWidget(row_i, 7)
position_model.status = widget.currentData()
position_model.id = int(self.tableWidget_line_positions.item(row_i, 8).text())
combox_line_type = self.tableWidget_line_positions.cellWidget(row_i, 7)
position_model.lineType = combox_line_type.currentData()
combox_status = self.tableWidget_line_positions.cellWidget(row_i, 8)
position_model.status = combox_status.currentData()
position_model.id = int(self.tableWidget_line_positions.item(row_i, 9).text())
position_model.order = row_i
position_model.lineId = int(self.tableWidget_line_positions.item(row_i, 10).text())
position_model.lineId = int(self.tableWidget_line_positions.item(row_i, 11).text())
position_models.append(position_model)
return position_models
def init_table_lines_data(self):
# self.tableWidget_lines.setItem(0, 0, QTableWidgetItem("线1"))
# self.tableWidget_lines.setItem(1, 0, QTableWidgetItem("线2"))
# self.tableWidget_lines.setItem()
self.selected_line_section = ''
self.table_line_changed = False
self.table_position_changed = False
self.tableWidget_lines.setRowCount(0)
for key, value in self.feedLine_dict.items():
self.tableWidget_lines.insertRow(self.tableWidget_lines.rowCount())
self.tableWidget_lines.setItem(self.tableWidget_lines.rowCount()-1, 0, QTableWidgetItem(key))
self.tableWidget_lines.setItem(self.tableWidget_lines.rowCount()-1, 1, QTableWidgetItem(value.name))
self.tableWidget_lines.setItem(self.tableWidget_lines.rowCount()-1, 2, QTableWidgetItem(value.id))
self.tableWidget_lines.setItem(self.tableWidget_lines.rowCount()-1, 2, QTableWidgetItem(str(value.id)))
show_pos_btn = QPushButton("显示路径")
show_pos_btn.clicked.connect(
lambda _, index=self.tableWidget_lines.rowCount()-1: self.show_lines_pos_data(index))
self.tableWidget_lines.setCellWidget(self.tableWidget_lines.rowCount() - 1, 3, show_pos_btn)
self.tableWidget_line_positions.setRowCount(0)
self.selected_line_section = ''
self.tableWidget_line_positions.setRowCount(0)
def show_lines_context_menu(self):
menu = QMenu(self)
@ -293,6 +300,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
result = msgBox.exec()
if result == QMessageBox.StandardButton.Ok:
self.send_save_lines_data()
return
else:
# 重新初始化·
self.init_table_lines_data()
@ -304,26 +312,27 @@ class MainWindow(QMainWindow, Ui_MainWindow):
def init_table_position(self):
#self.tableWidget_line_positions.setRowCount(5)
self.tableWidget_line_positions.setColumnCount(12)
self.tableWidget_line_positions.setColumnCount(13)
self.tableWidget_line_positions.setHorizontalHeaderLabels(["节段","X","Y","Z","U","V","W","类型","序号","排序","线号","操作2"])
self.tableWidget_line_positions.setHorizontalHeaderLabels(["节段","X","Y","Z","U","V","W","运动类型","点位类型","序号","排序","线号","操作2"])
self.tableWidget_line_positions.setSelectionBehavior(QTableWidget.SelectRows)
self.tableWidget_line_positions.setColumnHidden(0, True)
self.tableWidget_line_positions.setColumnHidden(8,True)
self.tableWidget_line_positions.setColumnHidden(9,True)
self.tableWidget_line_positions.setColumnHidden(10, True)
self.tableWidget_line_positions.setColumnHidden(11, True)
self.tableWidget_line_positions.setAutoScroll(True)
self.tableWidget_line_positions.verticalHeader().hide()
# 自动调整列宽
header = self.tableWidget_line_positions.horizontalHeader()
header.setSectionResizeMode(0, PySide6.QtWidgets.QHeaderView.ResizeMode.Stretch)
header.setSectionResizeMode(0, PySide6.QtWidgets.QHeaderView.ResizeMode.Interactive)
self.tableWidget_line_positions.setHorizontalScrollBarPolicy(PySide6.QtCore.Qt.ScrollBarPolicy.ScrollBarAsNeeded)
for i in range(12):
header.setSectionResizeMode(i, PySide6.QtWidgets.QHeaderView.ResizeMode.Stretch)
for i in range(13):
header.setSectionResizeMode(i, PySide6.QtWidgets.QHeaderView.ResizeMode.Interactive)
self.tableWidget_line_positions.setContextMenuPolicy(PySide6.QtCore.Qt.ContextMenuPolicy.CustomContextMenu)
self.tableWidget_line_positions.customContextMenuRequested.connect(self.show_positions_context_menu)
def init_table_positions_data(self,position_list):
self.tableWidget_line_positions.setRowCount(0)
for index,position_model in enumerate(position_list):
self.tableWidget_line_positions.insertRow(self.tableWidget_line_positions.rowCount())
self.set_positionModel_to_tabel(index,position_model)
@ -347,13 +356,21 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.table_position_changed = True
def add_new_position(self):# 选中添加
if self.selected_line_section == '': return
config_reader = configparser.ConfigParser()
config_reader.read(Constant.feedLine_set_file, encoding = 'utf-8')
position_model = PositionModel(999)
ids = []
for j in range(self.tableWidget_line_positions.rowCount()):
ids.append(int(self.tableWidget_line_positions.item(j, 9).text()))
for i in range(1,Constant.MAX_Position_num):
if not config_reader.has_section(f"{Constant.position_set_section}{i}"):
if ids.__contains__(i):
continue
position_model = PositionModel(i)
break
if position_model.id == 999:
print("d")
if self.tableWidget_line_positions.currentRow()==-1:
row_i = self.tableWidget_line_positions.rowCount()
else:
@ -362,8 +379,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.tableWidget_line_positions.insertRow(row_i)
position_model.order = 0
position_model.status = FeedStatus.FStart
position_model.status = 1
line_model = self.feedLine_dict.get(self.selected_line_section)
line_id = line_model.id
position_model.lineId = line_id
@ -371,7 +387,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.table_line_changed = True
def tabel_get_position(self,row_i):
self.set_position_to_tabel(row_i,self.status_address.getRealPosition())
self.set_position_tabel(row_i,self.status_address.getRealPosition())
self.table_line_changed = True
def tabel_delete_position(self,row_i):
@ -381,8 +397,16 @@ class MainWindow(QMainWindow, Ui_MainWindow):
def set_positionModel_to_tabel(self,row_i,position_model:PositionModel):
self.tableWidget_line_positions.setItem(row_i,0,QTableWidgetItem(str(position_model.section)))
self.set_position_tabel(row_i,position_model.get_position())
combox_lineType = QComboBox()
combox_lineType.addItem("直线",0)
combox_lineType.addItem("曲线中间点",2)
combox_lineType.addItem("曲线终点",3)
combox_lineType.setCurrentIndex(combox_lineType.findData(position_model.lineType))
combox_lineType.currentIndexChanged.connect(self.send_table_position_status_changed)
self.tableWidget_line_positions.setCellWidget(row_i, 7, combox_lineType)
combox = QComboBox()
combox.addItem("初始化点", 1)
combox.addItem("初始化点", 2)
combox.addItem("中间点", 3)
combox.addItem("相机/待抓点",4)
combox.addItem("抓取",5)
@ -390,18 +414,18 @@ class MainWindow(QMainWindow, Ui_MainWindow):
combox.addItem("破带点2",7)
combox.addItem("震动点",8)
combox.addItem("扔带点",9)
combox.setCurrentIndex(position_model.status)
combox.setCurrentIndex(combox.findData(position_model.status))
combox.currentIndexChanged.connect(self.send_table_position_status_changed)
self.tableWidget_line_positions.setCellWidget(row_i, 7, combox)
self.tableWidget_line_positions.setCellWidget(row_i, 8, combox)
#self.tableWidget_line_positions.setItem(row_i, 7, QTableWidgetItem(str(position_model.status)))
self.tableWidget_line_positions.setItem(row_i, 8, QTableWidgetItem(str(position_model.id)))
self.tableWidget_line_positions.setItem(row_i, 9, QTableWidgetItem(str(position_model.order)))
self.tableWidget_line_positions.setItem(row_i, 10, QTableWidgetItem(str(position_model.lineId)))
self.tableWidget_line_positions.setItem(row_i, 9, QTableWidgetItem(str(position_model.id)))
self.tableWidget_line_positions.setItem(row_i, 10, QTableWidgetItem(str(position_model.order)))
self.tableWidget_line_positions.setItem(row_i, 11, QTableWidgetItem(str(position_model.lineId)))
get_pos_btn = QPushButton("获取点位")
get_pos_btn.clicked.connect(
lambda _, index=row_i: self.tabel_get_position(index))
self.tableWidget_line_positions.setCellWidget(row_i, 11, get_pos_btn)
self.tableWidget_line_positions.setCellWidget(row_i, 12, get_pos_btn)
# del_pos_btn = QPushButton("删除点位")
# del_pos_btn.clicked.connect(lambda _, index=row_i: self.tabel_delete_position(index))
# self.tableWidget_line_positions.setCellWidget(row_i, 12, del_pos_btn)
@ -781,9 +805,9 @@ class MainWindow(QMainWindow, Ui_MainWindow):
return
log.log_message(logging.INFO, '第一层确认生产')
has_person, _ = self.detection_person.get_person()
if has_person:
has_person = False
# has_person, _ = self.detection_person.get_person() # TODO
if has_person: # TODO
# self.feeding.pause = True
# self.send_pause_command(True)
#
@ -800,7 +824,8 @@ class MainWindow(QMainWindow, Ui_MainWindow):
num = self.horizontalSlider_feedingNum.maximum()
line_head = self.comboBox_lineIndex.currentData()
self.command_quene.put(FeedCommand(FeedingConfig(num, self.feedLine_dict[line_head], self.feeding.robotClient.photo_locs[:])))
self.command_quene.put(FeedCommand(FeedingConfig(num, FeedLine(self.feedLine_dict[line_head].id,self.feedLine_dict[line_head].name,self.feedLine_dict[line_head].positions), self.feeding.robotClient.photo_locs[:])))
self.stackedWidget_num.setCurrentIndex(1)
self.set_run_status_button(True)
self.feeding.pause = False
@ -1166,7 +1191,8 @@ class MainWindow(QMainWindow, Ui_MainWindow):
if isinstance(command, FeedCommand) and command.status == Status.Prepareing:
if self.feeding.feedStatus == FeedStatus.FNone:
self.feeding.feedConfig = command.feed_config
self.feeding.feedStatus = FeedStatus.FCheck
self.feeding.feedStatus = FeedStatus.FStart
# self.feeding.feed_Mid_Status = FeedMidStatus.FMid_Start
command.status = Status.Runing
try:
@ -1315,7 +1341,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.set_sign_status(self.label_sign_take, self.pushButton_sign_take, True)
self.set_sign_status(self.label_sign_feed, self.pushButton_sign_feed, False)
return
if sign <= FeedStatus.FBroken:
if sign <= FeedStatus.FBroken1:
self.set_sign_status(self.label_sign_go, self.pushButton_sign_go, False)
self.set_sign_status(self.label_sign_photo, self.pushButton_sign_photo, False)
self.set_sign_status(self.label_sign_take, self.pushButton_sign_take, False)

61
test_ui.py
View File

@ -1,61 +1,10 @@
from PySide6.QtWidgets import QApplication, QWidget, QTableWidget, QTableWidgetItem, QComboBox, QPushButton, QVBoxLayout, QHBoxLayout
from PySide6.QtCore import Qt
from enum import Enum
class TableWithComboAndButtons(QWidget):
def __init__(self):
super().__init__()
self.setWindowTitle("表格：两列按钮，某一列下拉框")
self.setGeometry(100, 100, 600, 400)
# 布局
layout = QVBoxLayout()
def keepA(a,b=1,c=2,d=3):
print(a)
print('Straight')
# 创建表格
self.table = QTableWidget(5, 3) # 5行，3列
self.table.setHorizontalHeaderLabels(["位置", "获取位置", "保存位置"])
keepA(2)
# 设置“位置”列为下拉框
for row in range(5):
combo = QComboBox()
combo.addItems(["位置 A", "位置 B", "位置 C"])
self.table.setCellWidget(row, 0, combo)
# 设置“获取位置”和“保存位置”列为按钮
for row in range(5):
get_button = QPushButton("获取位置")
get_button.clicked.connect(self.get_location)
self.table.setCellWidget(row, 1, get_button)
save_button = QPushButton("保存位置")
save_button.clicked.connect(self.save_location)
self.table.setCellWidget(row, 2, save_button)
# 将表格添加到布局中
layout.addWidget(self.table)
self.setLayout(layout)
def get_location(self):
# 获取当前行的“位置”列的内容
selected_row = self.table.currentRow()
if selected_row >= 0:
location = self.table.cellWidget(selected_row, 0).currentText()
print(f"获取位置: {location}")
else:
print("没有选中任何行")
def save_location(self):
# 获取当前行的“位置”列的内容并保存
selected_row = self.table.currentRow()
if selected_row >= 0:
location = self.table.cellWidget(selected_row, 0).currentText()
print(f"保存位置: {location}")
self.table.setItem(selected_row, 0, QTableWidgetItem(f"已保存 - {location}"))
else:
print("没有选中任何行")
if __name__ == "__main__":
app = QApplication([])
window = TableWithComboAndButtons()
window.show()
app.exec()