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-04 23:42:52 +08:00
parent aa93685862
commit db283fbe71
10 changed files with 140 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
COM/COM_Robot.py
View File

@ -16,7 +16,7 @@ class DetectType(Enum):
class RobotClient(TCPClient):
def __init__(self, ip, port, photo_locs,command_quene, status_model:DataAddress,con_ios, time_delay_take,time_delay_put,time_delay_shake):
def __init__(self, ip, port, photo_locs,command_quene, status_model:DataAddress,con_ios, time_delay_take,time_delay_put,time_delay_shake,origin_position):
super().__init__(ip, port)
self.command_quene = command_quene
self.status_model = status_model
@ -27,6 +27,7 @@ class RobotClient(TCPClient):
self.time_delay_put = time_delay_put
self.time_delay_shake = time_delay_shake
self.type_detection = DetectType.EyeOutHand
self.origin_position = origin_position
def add_sendQuene(self,command): #后面 命令分等级，紧急命令直接执行
self.command_quene.put(command)

59
CU/Feeding.py
View File

@ -6,10 +6,12 @@ from PyQt5.QtWidgets import QMessageBox
import Constant
import Expection
from Model.FeedModel import PositionModel
from Model.Position import Real_Position, Detection_Position
from enum import Enum, IntEnum
from COM.COM_Robot import RobotClient, DetectType
from Model.RobotModel import CMDInstructRequest, MoveType
from Util.util_math import get_distance
from Util.util_time import CRisOrFall
from Vision.camera_coordinate_dete import Detection
from Util.util_log import log
@ -134,7 +136,6 @@ class FeedLine:
class FeedingConfig:
def __init__(self, num: int, feedLine: FeedLine, photo_locs):
self.num = num
@ -162,6 +163,7 @@ class Feeding:
self.cRis_photo = CRisOrFall()
self.feed_Mid_Status = FeedMidStatus.FMid_Start
self.is_reverse = False
self.run_reverse = False
pass
def init_detection_image(self):
@ -211,16 +213,17 @@ class Feeding:
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:
if not self.robotClient.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):
if self.is_reverse and self.robotClient.origin_position.compare(real_position):
self.feedStatus = FeedStatus.FNone
self.is_reverse = False
return
# TODO 获取目标位置
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,
# save_img_point=0, Height_reduce=30, width_reduce=30)
self.feed_Mid_Status = FeedMidStatus.FMid_Start
@ -366,9 +369,53 @@ class Feeding:
log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
self.next_position()
def return_original_position(self):
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
start_index = -1
# for index in range(len(self.feedConfig.feedLine.positions)):
# if self.feedConfig.feedLine.positions[index].status == 2:
# start_index = index
pos_index = -1
pos_near_index = -1
reversed_positions = []
for index, pos_model in enumerate(self.feedConfig.feedLine.positions):
if pos_model.get_position().compare(real_position):
pos_index = index
break
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):
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]
else:
return False
else:
reversed_positions = self.feedConfig.feedLine.positions[::pos_index]
reversed_positions = list(reversed(reversed_positions))
self.reverse_index = 0
while self.run_reverse and reversed_positions!=len(reversed_positions):
pos_model = reversed_positions[self.reverse_index]
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())
else:
self.sendTargPosition(real_position=pos_model.get_position())
self.reverse_index = self.reverse_index + 1
return True
def send_emergency_sound(self):
self.sendIOControl(Constant.IO_EmergencyPoint, 1)

3
Constant.py
View File

@ -8,6 +8,7 @@ manual_adjust_accuracy = 1
speed = 10
shake_speed = 20
debug_speed=10
return_speed = 10
feedLine_set_section = 'FeedLine'
position_set_section = 'Position'
feedLine_set_file = f'.{os.sep}Config{os.sep}feedLine.ini'
@ -57,6 +58,8 @@ str_feed_io_control = '发送IO控制: '
str_feed_safe_error_msgbox = '未在安全位置，请先复位！'
str_feed_shake = '摇摆'
str_feed_start_error = '请先复位机械臂'
str_feed_return_original_position_fail = '机械臂未按照实际路线点移动，未能寻找到适配路径点位'
str_feed_reset_no_line_error = '未定义的线段'
str_sys_start = '进入系统'
str_sys_exit = '退出系统'
str_sys_switch_tool = '切换到工具坐标'

2
MainWin.ui
View File

@ -907,7 +907,7 @@ background-color: rgb(13, 17, 40);</string>
</sizepolicy>
</property>
<property name="currentIndex">
<number>5</number>
<number>2</number>
</property>
<widget class="QWidget" name="page_3">
<layout class="QVBoxLayout" name="verticalLayout_7" stretch="1">

6
Model/FeedModel.py
View File

@ -53,6 +53,12 @@ class PositionModel:
self.lineType = 0
self.section = f'Position{index}'
pass
def init_position(self,real_pos:Real_Position):
self.set_position(real_pos)
self.id = 0
self.order = 0
self.status = 3
def read_position_model(self,config_reader,index:int):
self.section = f'Position{index}'

8
Seting.ini
View File

@ -55,6 +55,14 @@ takedelay = 0.2
putdelay = 0.1
shakedelay = 1.0
[Origin]
X = 0.0
Y = 0.0
Z = 0.0
U = 0.0
V = 0.0
W = 0.0
[Camera_Feed]
ipaddress = 127.0.0.1

11
Util/util_math.py Normal file
View File

@ -0,0 +1,11 @@
import math
def get_distance(p1, p2):
"""
计算两点间的距离
:param p1:
:param p2:
:return:
"""
return math.sqrt((p1.X - p2.X) ** 2 + (p1.Y - p2.Y) ** 2+ (p1.Z - p2.Z)**2)

39
main.py
View File

@ -3,6 +3,7 @@ import json
import logging
import queue
import sys
import threading
from multiprocessing import Process
import traceback
@ -38,6 +39,7 @@ from CU.Command import Status
from Util.util_log import log
from Vision.detect_person import DetectionPerson
from ui_MainWin import Ui_MainWindow
from view.ResetView import StopDialog
class MainWindow(QMainWindow, Ui_MainWindow):
@ -521,6 +523,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.lineEdit_num.setValidator(int_validator)
# self.horizontalSlider_J1.sliderReleased
self.pushButton_startFeed.clicked.connect(self.send_startFeed_button_click)
@ -564,6 +567,13 @@ class MainWindow(QMainWindow, Ui_MainWindow):
float(self.configReader.get('Robot_Feed', 'photo_u5')),float(self.configReader.get('Robot_Feed', 'photo_v5')),float(self.configReader.get('Robot_Feed', 'photo_w5'))
)
]
origin_position = Real_Position(float(self.configReader.get('Origin', 'X')),
float(self.configReader.get('Origin', 'Y')),
float(self.configReader.get('Origin', 'Z')),
float(self.configReader.get('Origin', 'U')),
float(self.configReader.get('Origin', 'V')),
float(self.configReader.get('Origin', 'W')))
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'))
@ -572,7 +582,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
time_delay_shake = float(self.configReader.get('Robot_Feed', 'shakeDelay'))
#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,[solenoid_valve1_addr, solenoid_valve2_addr, solenoid_valve3_addr],time_delay_take,time_delay_put,time_delay_shake)
self.robotClient = RobotClient(ip, port, photo_locs, self.command_position_quene, self.status_address,[solenoid_valve1_addr, solenoid_valve2_addr, solenoid_valve3_addr],time_delay_take,time_delay_put,time_delay_shake,origin_position)
self.feeding = Feeding(self.robotClient, self.detection) # 临时
self.last_time = time.time()
self.remain_lineName = self.configReader.get('Robot_Feed', 'remain_lineName')
@ -1109,7 +1119,28 @@ 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
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)
# 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}'
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),
self.feeding.robotClient.photo_locs[:])
else:
log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error)
return
else:
log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error)
return
thread = threading.Thread(target=self.feeding.return_original_position())
thread.start()
dialog_reset = StopDialog()
dialog_reset.stop_thread_signal.connect(self.stop_reset_thread)
def stop_reset_thread(self):
self.feeding.run_reverse = False
self.send_clear_auto_command()
self.send_emergency_alarm_command()
def send_tabelFeedSet_itemChanged(self, item):
row = item.row()
@ -1190,10 +1221,14 @@ class MainWindow(QMainWindow, Ui_MainWindow):
command = self.command_quene.get()
if isinstance(command, FeedCommand) and command.status == Status.Prepareing:
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)
self.feeding.feedConfig = command.feed_config
self.feeding.feedStatus = FeedStatus.FStart
# self.feeding.feed_Mid_Status = FeedMidStatus.FMid_Start
command.status = Status.Runing
self.record_remain_num()
try:
self.feeding.run()

13
test_ui.py
View File

@ -1,10 +1,3 @@
from enum import Enum
def keepA(a,b=1,c=2,d=3):
print(a)
print('Straight')
keepA(2)
my_list = [1, 2, 3, 4, 5]
list_slice = list(reversed(my_list[:2]))
print(list_slice) # 输出: [5, 4, 3, 2, 1]

16
view/ResetView.py Normal file
View File

@ -0,0 +1,16 @@
from PyQt5.QtWidgets import QDialog
from PySide6.QtCore import Signal, Slot
from view.ui_Dialog_Reset import Ui_Dialog_reset
class StopDialog(QDialog, Ui_Dialog_reset):
stop_thread_signal = Signal() # 通知主线程停止信号
def __init__(self):
super(Ui_Dialog_reset, self).__init__()
self.setupUi(self)
self.pushButton_stopFeed.clicked.connect(self.emit_stop_signal)
@Slot()
def emit_stop_signal(self):
self.stop_thread_signal.emit()
self.close()