xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update 完成所有投料逻辑，IO点位控制，急停控制

Browse Source
This commit is contained in:
FrankCV2048
2024-10-29 23:06:25 +08:00
parent 5500ac7f71
commit e5ce0dc31d
11 changed files with 156 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
COM/COM_Robot.py
View File

@ -9,12 +9,13 @@ from Util.util_log import log
class RobotClient(TCPClient):
def __init__(self, ip, port, photo_locs,command_quene, status_model:DataAddress):
def __init__(self, ip, port, photo_locs,command_quene, status_model:DataAddress,con_ios):
super().__init__(ip, port)
self.command_quene = command_quene
self.status_model = status_model
self.errorCommands = {}
self.photo_locs = photo_locs
self.con_ios = con_ios
def add_sendQuene(self,command): #后面 命令分等级，紧急命令直接执行
self.command_quene.put(command)
return

92
CU/Feeding.py
View File

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

8
Constant.py
View File

@ -1,5 +1,7 @@
import os
IO_EmergencyPoint = 3
bag_height = 10 # 一袋的高度
position_accuracy = 0.05
manual_adjust_accuracy = 1
speed = 10
@ -30,6 +32,11 @@ str_feed_broken_bag = '划袋'
str_feed_drop = '移动扔空袋'
str_feed_takePhoto_fail = '识别图像失败'
str_feed_takePhoto_success = '识别图像成功'
str_feed_takePhoto_new_line = '新的一排袋识别'
str_feed_takePhoto_line = '一排袋最高的识别'
str_feed_takePhoto_front_finish = '零星袋完成'
str_feed_takePhoto_front = '零星袋识别'
str_feed_takePhoto_move = '移动到抓料位置'
str_feed_covert_success = '转换坐标成功'
str_feed_covert_fail = '转换坐标失败'
str_feed_error = '未知错误'
@ -48,6 +55,7 @@ str_sys_setFeedNum = '设置投料次数'
str_sys_feedNum_sub = '减少投料次数'
str_sys_feedNum_add = '增加投料次数'
str_sys_log_feedNum ='记录袋数失败'
str_sys_emergencyStop = '按下急停'
str_tcp_robot_connect_fail = '连接失败'
str_tcp_robot_connect_success = '连接成功'
str_tcp_robot_data_error = '数据解析错误'

2
MainWin.ui
View File

@ -4350,7 +4350,7 @@ background-color: #499c8a;
</widget>
</item>
<item row="2" column="0">
<widget class="QPushButton" name="pushButton_17">
<widget class="QPushButton" name="pushButton_emergency">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Expanding">
<horstretch>0</horstretch>

3
Model/Position.py
View File

@ -46,3 +46,6 @@ class Real_Position(Position):
self.V = V
self.W = W
return self
def to_string(self):
return "X:{:.3f},Y:{:.3f},Z:{:.3f},U:{:.3f},V:{:.3f},W:{:.3f}".format(self.X,self.Y,self.Z,self.U,self.V,self.W)

2
Model/RobotModel.py
View File

@ -162,7 +162,7 @@ class CMDInstructRequest:
def __init__(self):
self.dsID = 'www.hc-system.com.HCRemoteCommand'
self.reqType = "AddRCC"
self.emptyList = '1'
self.emptyList = '1' #清空机械臂的远程命令数据
self.instructions = []
def toString(self):

8
README.md Normal file
View File

@ -0,0 +1,8 @@
# 配置环境
- 设置相机的IP地址 Seting.ini
- 设置机械臂的IP地址 Seting.ini
- 设置袋高 Constant.py bag_height
- 设置五个拍照位置 界面
- 设置N个固定位置 界面
- 设置速度 界面
- 转换矩阵的R矩阵的设置的常数需要确定

7
Seting.ini
View File

@ -46,8 +46,11 @@ photo_u5 = 0
photo_v5 = 0
photo_w5 = 0
linecount = 2
remain_linename = 2
remain_count = 1
remain_linename = 0
remain_count = 0
solenoid_valve1_addr=0
solenoid_valve2_addr=0
solenoid_valve3_addr=0
[Camera_Feed]
ipaddress = 127.0.0.1

35
main.py
View File

@ -155,6 +155,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.pushButton_set_p5.clicked.connect(self.set_p5_button_click)
self.pushButton_clearAlarm.clicked.connect(self.send_clear_alarm_command)
self.pushButton_emergency.clicked.connect(self.send_emergency_alarm_command)
self.pushButton_reset.clicked.connect(self.send_reset_button_click)
self.pushButton_speed.setText(str(Constant.speed))
@ -221,9 +222,12 @@ class MainWindow(QMainWindow, Ui_MainWindow):
int(self.configReader.get('Robot_Feed', 'photo_u5')),int(self.configReader.get('Robot_Feed', 'photo_v5')),int(self.configReader.get('Robot_Feed', 'photo_w5'))
)
]
solenoid_valve1_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve1_addr'))
solenoid_valve2_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve2_addr'))
solenoid_valve3_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve3_addr'))
#TODO
#dropDelay_time = int(self.configReader.get('Robot_Feed', 'dropDelay_time'))
self.robotClient = RobotClient(ip, port, photo_locs, self.command_position_quene, self.status_address)
self.robotClient = RobotClient(ip, port, photo_locs, self.command_position_quene, self.status_address,[solenoid_valve1_addr, solenoid_valve2_addr, solenoid_valve3_addr])
self.feeding = Feeding(self.robotClient, self.detection) # 临时
self.last_time = time.time()
self.remain_lineName = self.configReader.get('Robot_Feed', 'remain_lineName')
@ -473,6 +477,9 @@ class MainWindow(QMainWindow, Ui_MainWindow):
return
def send_startFeed_button_click(self):
if self.feeding.feedStatus != FeedStatus.FNone:
self.show_infomessage_box("正在执行")
return
# 触发自动运行
if self.robotClient.status_model.curMode != 7:
self.send_switch_tool_command()
@ -1013,7 +1020,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
def set_run_status_button(self, isRuning: bool):
self.pushButton_pauseFeed.setText("暂停")
##self.pushButton_startFeed.setEnabled(False)
# if isRuning:
# self.pushButton_pauseFeed.show()
@ -1026,12 +1033,6 @@ class MainWindow(QMainWindow, Ui_MainWindow):
def send_clear_auto_command(self):
clear_command = CMDInstructRequest()
io_instruction= Instruction()
io_instruction.IO = True
io_instruction.point=0 #{"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]}
io_instruction.delay=1
clear_command.dsID = 'HCRemoteCommand'
clear_command.instructions.append(io_instruction)
request_command = clear_command.toString()
log.log_message(logging.INFO, Constant.str_sys_clearAlarm)
self.command_quene.put(request_command)
@ -1071,15 +1072,19 @@ class MainWindow(QMainWindow, Ui_MainWindow):
print(request_command)
self.robotClient.add_sendQuene(request_command)
def send_clear_alarm_command(self, pause: bool):
switch_command = CMDRequest()
switch_command.cmdData.append("modifyGSPD")
switch_command.cmdData.append("2")
request_command = switch_command.toString()
print(request_command)
def send_emergency_alarm_command(self):
stop_command = CMDRequest()
stop_command.cmdData.append("actionStop")
stop_command.cmdData.append("1")
request_command = stop_command.toString()
self.robotClient.add_sendQuene(request_command)
self.feeding.send_emergency_sound()
log.log_message(logging.INFO, Constant.str_sys_emergencyStop)
#TODO
def send_clear_alarm_command(self, pause: bool):
self.send_start_tool_command()
pause_command = CMDRequest()
pause_command.cmdData.append("clearAlarmContinue")
pause_command.cmdData.append("1")

24
test7.py Normal file
View File

@ -0,0 +1,24 @@
class Position:
def __init__(self):
self.X = 0.0
self.Y = 0.0
self.Z = 0.0
self.U = 0.0
self.V = 0.0
self.W = 0.0
def init_data(self, X, Y, Z, U, V, W):
self.X = X
self.Y = Y
self.Z = Z
return self
detect_pos_list = []
detect_pos_list.append(Position().init_data(0.0, 0.0, 3, 0.0, 0.0, 0.0))
detect_pos_list.append(Position().init_data(0.0, 0.0, 1, 0.0, 0.0, 0.0))
detect_pos_list.append(Position().init_data(0.0, 0.0, 8, 0.0, 0.0, 0.0))
z_diff, max_z_index = (lambda pts: (
max(pts, key=lambda p: p.Z).Z - min(pts, key=lambda p: p.Z).Z,
pts.index(max(pts, key=lambda p: p.Z))
))(detect_pos_list)
print(z_diff, max_z_index)

16
ui_MainWin.py
View File

@ -2740,11 +2740,11 @@ class Ui_MainWindow(object):
self.gridLayout_2.addWidget(self.pushButton_pauseFeed, 0, 1, 1, 2)
self.pushButton_17 = QPushButton(self.frame_21)
self.pushButton_17.setObjectName(u"pushButton_17")
sizePolicy5.setHeightForWidth(self.pushButton_17.sizePolicy().hasHeightForWidth())
self.pushButton_17.setSizePolicy(sizePolicy5)
self.pushButton_17.setStyleSheet(u"*{\n"
self.pushButton_emergency = QPushButton(self.frame_21)
self.pushButton_emergency.setObjectName(u"pushButton_emergency")
sizePolicy5.setHeightForWidth(self.pushButton_emergency.sizePolicy().hasHeightForWidth())
self.pushButton_emergency.setSizePolicy(sizePolicy5)
self.pushButton_emergency.setStyleSheet(u"*{\n"
"background-color: #FFF000;\n"
"font: 9pt \"\u6977\u4f53\";\n"
"border-radius: 10px;\n"
@ -2756,9 +2756,9 @@ class Ui_MainWindow(object):
"\n"
"")
icon10 = QIcon(QIcon.fromTheme(QIcon.ThemeIcon.WindowClose))
self.pushButton_17.setIcon(icon10)
self.pushButton_emergency.setIcon(icon10)
self.gridLayout_2.addWidget(self.pushButton_17, 2, 0, 1, 1)
self.gridLayout_2.addWidget(self.pushButton_emergency, 2, 0, 1, 1)
self.pushButton_clearAlarm = QPushButton(self.frame_21)
self.pushButton_clearAlarm.setObjectName(u"pushButton_clearAlarm")
@ -3136,7 +3136,7 @@ class Ui_MainWindow(object):
self.pushButton_SubNum.setText(QCoreApplication.translate("MainWindow", u"\u6263\u4e00\u888b", None))
self.pushButton_startFeed.setText(QCoreApplication.translate("MainWindow", u"\u542f\u52a8", None))
self.pushButton_pauseFeed.setText(QCoreApplication.translate("MainWindow", u"\u6682\u505c", None))
self.pushButton_17.setText(QCoreApplication.translate("MainWindow", u"\u6025\u505c", None))
self.pushButton_emergency.setText(QCoreApplication.translate("MainWindow", u"\u6025\u505c", None))
self.pushButton_clearAlarm.setText(QCoreApplication.translate("MainWindow", u"\u6e05\u9664\u62a5\u8b66", None))
self.pushButton_stopFeed.setText(QCoreApplication.translate("MainWindow", u"\u505c\u6b62", None))
self.pushButton_reset.setText(QCoreApplication.translate("MainWindow", u"\u590d\u4f4d", None))