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:
Gogs
2024-12-08 16:10:59 +08:00
parent d4fcb79394
commit 7c606caa7b
5 changed files with 196 additions and 353 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
CU/Feeding.py
View File

@ -3,6 +3,7 @@ import time
import cv2
from PyQt5.QtWidgets import QMessageBox
from jedi.debug import speed
import Constant
import Expection
@ -58,17 +59,17 @@ class FeedLine:
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]
pos = self.feeding_to_end[self.feeding2end_pos_index-1]
return pos
def get_current_take_position(self):
pos = self.start_to_take[self.start2take_pos_index]
pos = self.start_to_take[self.start2take_pos_index-1]
return pos
def get_current_start_position(self):
pos = self.origin_to_start[self.origin2start_pos_index]
pos = self.origin_to_start[self.origin2start_pos_index-1]
return pos
def get_next_feed_position(self,reverse:bool=False):
@ -187,11 +188,14 @@ class Feeding:
if self.feedConfig == None:
self.feedStatus = FeedStatus.FNone
elif self.feedConfig.num == 0 and self.feedStatus!=FeedStatus.FReverse:
if self.feedConfig !=None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(real_position):
self.feedStatus = FeedStatus.FNone
self.is_reverse = False
if self.feedStatus == FeedStatus.FNone or self.pause:
return
elif self.feedStatus == FeedStatus.FCheck:
log.log_message(logging.INFO, Constant.str_feed_check)
# 1, 检查是否是三列
@ -209,7 +213,9 @@ class Feeding:
self.feed_Mid_Status = FeedMidStatus.FMid_Start
else:
self.feed_Mid_Status = FeedMidStatus.FMid_Take
self.next_position(self.is_reverse)
if self.feedConfig.feedLine.start_to_take[0].get_position().compare(real_position):
self.next_position(self.is_reverse)
elif self.feedStatus == FeedStatus.FStart:
log.log_message(logging.INFO, Constant.str_feed_start)
@ -217,11 +223,13 @@ class Feeding:
QMessageBox.information(None, "提示", Constant.str_feed_start_error)
self.feedStatus = FeedStatus.FNone
return
if self.is_reverse and self.robotClient.origin_position.compare(real_position):
if self.is_reverse:
self.feedStatus = FeedStatus.FNone
self.is_reverse = False
return
self.feedConfig.feedLine.get_position_list()
self.feedConfig.feedLine.set_take_position(real_position)
# TODO 获取目标位置
# self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,
@ -243,8 +251,11 @@ class Feeding:
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.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.next_position(self.is_reverse)
return
if self.robotClient.type_detection == DetectType.EyeOutHand:
@ -324,9 +335,9 @@ class Feeding:
self.take_no_photo = False
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)
# 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)
@ -364,9 +375,9 @@ class Feeding:
log.log_message(logging.INFO, Constant.str_feed_drop)
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)
# 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 获取目标位置
@ -393,7 +404,7 @@ class Feeding:
pos_index = -1
pos_near_index = -1
reversed_positions = []
for index, pos_model in enumerate(self.feedConfig.feedLine.positions):
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
if pos_model.get_position().compare(real_position):
pos_index = index
break
@ -401,28 +412,37 @@ class Feeding:
if pos_index == -1:
log.log_message(logging.ERROR, Constant.str_feed_return_original_position_fail)
min_distance = 99999999
for index, pos_model in enumerate(self.feedConfig.feedLine.positions):
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)
pos_near_index = index
if pos_near_index != -1:
reversed_positions = self.feedConfig.feedLine.positions[::pos_near_index+1]
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_near_index+1]
else:
return False
else:
reversed_positions = self.feedConfig.feedLine.positions[::pos_index]
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_index]
reversed_positions = list(reversed(reversed_positions))
self.reverse_index = 0
self.send_emergency_sound()
while self.run_reverse and reversed_positions!=len(reversed_positions):
while self.run_reverse and self.reverse_index!=len(reversed_positions):
if self.reverse_index!=0 and not real_position.compare(reversed_positions[self.reverse_index-1]):
continue
print(f'复位第{self.reverse_index}') #todo 缺少比对
pos_model = reversed_positions[self.reverse_index]
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
print('跳过抓袋点位')
pos_model = reversed_positions[self.reverse_index + 1]
# TODO take节点判断
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())
real_position1=pos_model1.get_position(),speed= self.robotClient.reset_speed)
self.reverse_index = self.reverse_index+2
else:
self.sendTargPosition(real_position=pos_model.get_position(),speed=self.robotClient.reset_speed)
self.reverse_index = self.reverse_index + 1
self.reverse_index = self.reverse_index + 1
self.send_emergency_stop()
return True
def send_emergency_sound(self):
@ -466,7 +486,7 @@ class Feeding:
instruction_command.instructions.append(position_instruction)
request_command = instruction_command.toString()
log_str = f'移动到位置:{"姿势直线"}：' \
log_str = f'移动到位置:{ "姿势直线" if move_type==MoveType.WORLD else "姿势曲线"}：' \
f'X:{position_instruction.m0}-' \
f'Y:{position_instruction.m1}-' \
f'Z:{position_instruction.m2}-' \
@ -474,6 +494,7 @@ class Feeding:
f'V:{position_instruction.m4}-' \
f'W:{position_instruction.m5}'
print(log_str)
try:
log.log_message(logging.INFO, log_str)
except:
@ -484,33 +505,39 @@ class Feeding:
def next_start(self,reverse=False):
print(f'原点到初始位置的第{self.feedConfig.feedLine.origin2start_pos_index}')
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(),speed=self.robotClient.feed_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.debug_speed, move_type=MoveType.Cure, real_position1=start_pos1.get_position())
else:
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.feed_speed)
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.debug_speed)
pass
def next_take(self,reverse=False):
print(f'初始位置到拍照点的第{self.feedConfig.feedLine.start2take_pos_index}')
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(),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.debug_speed)
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.debug_speed)
pass
def next_Feed(self,reverse=False):
print(f'拍照到抛料回来的第{self.feedConfig.feedLine.feeding2end_pos_index}')
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(),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.debug_speed)
else:
self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.debug_speed)
def get_current_position(self):
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:

406
Config/FeedLine.ini
View File

@ -6,13 +6,26 @@ name = 反应釜1
id = 2
name = 反应釜2
[Position1]
x = 711.544373
y = 2058.108887
z = 1652.191772
u = 3.285049
v = 25.047607
w = -102.662521
id = 1
order = 1
lineid = 1
status = 2
linetype = 0
[Position2]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
x = 421.397
y = 2095.76
z = 1403.803
u = 2.787
v = 17.14
w = -95.084
id = 2
order = 0
lineid = 1
@ -20,331 +33,132 @@ status = 3
linetype = 2
[Position3]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
x = 116.779472
y = 1554.860718
z = 598.915833
u = 4.423964
v = 15.661633
w = -80.555466
id = 3
order = 2
lineid = 1
status = 2
status = 4
linetype = 0
[Position4]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
x = 230.750946
y = 1330.444702
z = 339.72052
u = 2.843005
v = 6.159603
w = -85.733009
id = 4
order = 4
order = 3
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
x = 116.779472
y = 1554.860718
z = 598.915833
u = 4.423964
v = 15.661633
w = -80.555466
id = 5
order = 6
order = 4
lineid = 1
status = 3
status = 5
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
[Position6]
x = 230.750946
y = 1330.444702
z = 339.72052
u = 2.843005
v = 6.159603
w = -85.733009
id = 6
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
[Position7]
x = 116.779472
y = 1554.860718
z = 598.915833
u = 4.423964
v = 15.661633
w = -80.555466
id = 7
order = 6
lineid = 1
status = 6
linetype = 0
[Position8]
x = 230.750946
y = 1330.444702
z = 339.72052
u = 2.843005
v = 6.159603
w = -85.733009
id = 8
order = 7
lineid = 1
status = 7
linetype = 0
[Position9]
x = 116.779472
y = 1554.860718
z = 598.915833
u = 4.423964
v = 15.661633
w = -80.555466
id = 9
order = 8
lineid = 1
status = 8
linetype = 0
[Position10]
x = 230.750946
y = 1330.444702
z = 339.72052
u = 2.843005
v = 6.159603
w = -85.733009
id = 10
order = 9
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
x = 116.779472
y = 1554.860718
z = 598.915833
u = 4.423964
v = 15.661633
w = -80.555466
id = 11
order = 9
order = 10
lineid = 1
status = 4
status = 9
linetype = 0
[Position12]
x = 7.0
y = 50.0
z = 1.0
u = 12.0
v = 0.0
w = 1.0
x = 230.750946
y = 1330.444702
z = 339.72052
u = 2.843005
v = 6.159603
w = -85.733009
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 = 124124
[Position23]
x = 0.0
y = 0.0
z = 0.0
u = 0.0
v = 0.0
w = 0.0
id = 23
order = 0
lineid = 3
status = 1
linetype = 0
[Position24]
x = 0.0
y = 0.0
z = 0.0
u = 0.0
v = 0.0
w = 0.0
id = 24
order = 1
lineid = 3
status = 1
linetype = 0
[Position25]
x = 0.0
y = 0.0
z = 0.0
u = 0.0
v = 0.0
w = 0.0
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

19
Seting.ini
View File

@ -56,18 +56,17 @@ putdelay = 0.1
shakedelay = 1.0
[Speed]
debug_speed=10
feed_speed=10
reset_speed=10
debug_speed = 50
feed_speed = 10
reset_speed = 35
[Origin]
X = 0.0
Y = 0.0
Z = 0.0
U = 0.0
V = 0.0
W = 0.0
x = 143.921
y = 2170.454
z = 1627.494
u = 3.234
v = 24.269
w = -87.403
[Camera_Feed]
ipaddress = 127.0.0.1

22
main.py
View File

@ -1005,7 +1005,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
log.log_message(logging.ERROR, Constant.str_sys_set_error+e)
pass
#self.pushButton_speed.setText(str(Constant.speed))
log.log_message(logging.INFO, Constant.str_sys_setSpeed + str(Constant.speed)+'|'+str(Constant.debug_speed))
log.log_message(logging.INFO, Constant.str_sys_setSpeed + str(self.robotClient.debug_speed)+'|'+str(self.robotClient.feed_speed))
pass
def send_get_safe_position_button_click(self):
@ -1134,13 +1134,17 @@ class MainWindow(QMainWindow, Ui_MainWindow):
def send_reset_button_click(self):
line_head = self.comboBox_lineIndex.currentData()
safe_position = self.feedLine_dict[line_head].safe_position
#safe_position = self.feedLine_dict[line_head].safe_position
# self.send_position_command(safe_position.X, safe_position.Y, safe_position.Z, safe_position.U, safe_position.V, safe_position.W,move_type=MoveType.WORLD)
if self.remain_lineName != '':
line_head = f'{Constant.feedLine_set_section}{self.remain_lineName}'
return_positions = self.feedLine_dict[line_head].positions
position_origin = PositionModel()
position_origin.init_position(self.robotClient.origin_position)
return_positions.insert(0,position_origin)
if self.feedLine_dict.keys().__contains__(line_head):
self.feeding.feedConfig= FeedingConfig(0, FeedLine(self.feedLine_dict[line_head].id, self.feedLine_dict[line_head].name,
self.feedLine_dict[line_head].positions),
return_positions),
self.feeding.robotClient.photo_locs[:])
else:
log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error)
@ -1239,10 +1243,11 @@ class MainWindow(QMainWindow, Ui_MainWindow):
if self.feeding.feedStatus == FeedStatus.FNone:
position_origin = PositionModel()
position_origin.init_position(self.robotClient.origin_position)
command.feed_config.feedLine.positions.insert(0,position_origin)
position_origin.status = 1
command.feed_config.feedLine.feed_positions.insert(0,position_origin)
self.feeding.feedConfig = command.feed_config
self.robotClient.send_emergency_sound()
time.sleep(5000)
time.sleep(5)
self.robotClient.send_emergency_stop()
self.feeding.feedStatus = FeedStatus.FStart
# self.feeding.feed_Mid_Status = FeedMidStatus.FMid_Start
@ -1271,6 +1276,8 @@ class MainWindow(QMainWindow, Ui_MainWindow):
# if result == QMessageBox.StandardButton.Cancel:
# return
log.log_message(logging.ERROR, '人员进入安全区')
time.sleep(3)
pass
def updateUI(self):
@ -1789,8 +1796,9 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.lineEdit_w5.setText(str(self.robotClient.photo_locs[4][5]))
pass
def updateUI_Base_Set(self):
self.lineEdit_speed_run.setText(str(Constant.speed))
self.lineEdit_speed_debug.setText(str(Constant.shake_speed))
self.lineEdit_speed_run.setText(str(self.robotClient.feed_speed))
self.lineEdit_speed_debug.setText(str(self.robotClient.debug_speed))
self.lineEdit_speed_reset.setText(str(self.robotClient.reset_speed))
self.lineEdit_solenoid1_addr.setText(str(self.robotClient.con_ios[0]))
self.lineEdit_solenoid2_addr.setText(str(self.robotClient.con_ios[1]))
self.lineEdit_solenoid3_addr.setText(str(self.robotClient.con_ios[2]))

19
test_ui.py
View File

@ -1,14 +1,9 @@
output_n = 7
io_bit = 5
from CU.Feeding import Feeding
from Model.Position import Real_Position
from Model.RobotModel import MoveType
def is_bit_set(n, position):
"""
检查整数 n 的第 position 位是否为 1
:param n: 整数
:param position: 从右往左数的位，0 表示最低位
:return: True 表示该位是 1，否则为 False
"""
return (n >> position) & 1 == 1
feeding = Feeding(None,None)
p_mid = Real_Position().init_position(20,20,20,30,20,20)
p_end = Real_Position().init_position(30,40,5,30,3,3)
print(is_bit_set(7,3))
feeding.sendTargPosition(p_mid,MoveType.Cure,5,p_end)