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-09 14:03:53 +08:00
parent f9b338f901
commit e43859b522
3 changed files with 139 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

118
CU/Feeding.py
View File

@ -4,20 +4,27 @@ import time
import cv2
from PyQt5.QtWidgets import QMessageBox
from jedi.debug import speed
from tifffile.tifffile import read_lsm_positions
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 Trace.handeye_calibration import getPosition
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
from Model.RobotModel import Instruction
class ResetStatus(Enum):
RNone = 0
RStart = 1
RRunging = 2
ROk =3
class FeedStatus(IntEnum):
@ -163,8 +170,15 @@ class Feeding:
self.cRis_photo = CRisOrFall()
self.feed_Mid_Status = FeedMidStatus.FMid_Start
self.is_reverse = False
# 复位集合
self.run_reverse = False
self.take_no_photo = False
self.reset_status = ResetStatus.RNone
self.reversed_positions = []
self.current_position = None
self.pos_index = -1
self.pos_near_index = -1
pass
def init_detection_image(self):
@ -220,7 +234,7 @@ class Feeding:
elif self.feedStatus == FeedStatus.FStart:
log.log_message(logging.INFO, Constant.str_feed_start)
if not self.robotClient.origin_position.compare(real_position) and not self.is_reverse:
QMessageBox.information(None, "提示", Constant.str_feed_start_error)
# QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
self.feedStatus = FeedStatus.FNone
return
if self.is_reverse:
@ -230,10 +244,13 @@ class Feeding:
self.feedConfig.feedLine.get_position_list()
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.feedConfig.feedLine.set_take_position(self.get_take_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
self.next_position(self.is_reverse)
@ -382,12 +399,87 @@ class Feeding:
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.feedLine.set_take_position(self.get_take_position())
self.feedConfig.num = self.feedConfig.num - 1
log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
self.next_position()
def run_reseet(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)
if self.reset_status == ResetStatus.RNone:
return
if self.reset_status == ResetStatus.RStart:
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
self.pos_index = -1
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):
self.pos_index = index
break
if self.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.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]
else:
return False
else:
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index]
self.reversed_positions = list(reversed(self.reversed_positions))
self.reverse_index = 0
self.send_emergency_sound()
self.current_position = PositionModel()
self.current_position.init_position(real_position)
self.reset_status = ResetStatus.RRunging
if self.reset_status == ResetStatus.RRunging:
if not real_position.compare(self.current_position.get_position()):
return
pos_model = self.reversed_positions[self.reverse_index]
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
print('跳过抓袋点位')
pos_model = self.reversed_positions[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.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.current_position = pos_model
self.reverse_index = self.reverse_index + 1
if self.reverse_index == len(self.reversed_positions):
self.reset_status = ResetStatus.ROk
if self.reset_status == ResetStatus.ROk:
self.reset_status = ResetStatus.RNone
self.send_emergency_stop()
def return_original_position(self):
self.run_reverse = True
real_position = Real_Position()
real_position.init_position(self.robotClient.status_model.world_0,
@ -425,8 +517,10 @@ class Feeding:
reversed_positions = list(reversed(reversed_positions))
self.reverse_index = 0
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(reversed_positions[self.reverse_index-1]):
if self.reverse_index!=0 and not real_position.compare(current_position.get_position()):
continue
print(f'复位第{self.reverse_index}') #todo 缺少比对
pos_model = reversed_positions[self.reverse_index]
@ -439,9 +533,11 @@ class Feeding:
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)
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)
current_position = pos_model
self.reverse_index = self.reverse_index + 1
self.send_emergency_stop()
return True
@ -502,8 +598,12 @@ class Feeding:
self.robotClient.add_sendQuene(request_command)
pass
def get_take_position(self):
_, img, xyz, uvw, points = 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.detection_image = img
target_position,noraml_base = getPosition(*xyz,*uvw,None,points)
position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
return position
def next_start(self,reverse=False):
print(f'原点到初始位置的第{self.feedConfig.feedLine.origin2start_pos_index}')
start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)

4
Seting.ini
View File

@ -1,7 +1,7 @@
[Main]
[Robot_Feed]
ipaddress = 192.168.20.4
ipaddress = 127.0.0.1
port = 502
j1_min = -150
j1_max = +150
@ -47,7 +47,7 @@ photo_v5 = 0.0
photo_w5 = 1.0
linecount = 2
remain_linename = 1
remain_count = 0
remain_count = 1
solenoid_valve1_addr = 3
solenoid_valve2_addr = 2
solenoid_valve3_addr = 10

42
main.py
View File

@ -23,7 +23,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, FeedMidStatus
from CU.Feeding import FeedLine, FeedingConfig, Feeding, FeedStatus, FeedMidStatus, ResetStatus
from Util.util_log import QTextEditLogger
from Util.util_time import CRisOrFall
from Vision.camera_coordinate_dete import Detection
@ -39,6 +39,9 @@ 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
#from view.ResetView import StopDialog
@ -542,7 +545,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
def init_Run(self):
self.robotClient = None
self.configReader = configparser.ConfigParser()
self.detection = None # Detection() #TODO 关闭图像
self.detection =Detection() #TODO 关闭图像
self.command_position_quene = Queue()
self.status_address = DataAddress()
self.feedLine_dict = {}
@ -1133,6 +1136,9 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.send_start_tool_command()
def send_reset_button_click(self):
# TODO 清楚痕迹
# TODO 开启自动
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)
@ -1149,16 +1155,18 @@ class MainWindow(QMainWindow, Ui_MainWindow):
else:
log.log_message(logging.ERROR, Constant.str_feed_reset_no_line_error)
return
self.feeding.reset_status = ResetStatus.RStart
dialog_reset = StopDialog()
dialog_reset.stop_thread_signal.connect(self.stop_reset_thread)
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)
# thread = threading.Thread(target=self.feeding.return_original_position())
# thread.start()
def stop_reset_thread(self):
self.feeding.run_reverse = False
self.feeding.reset_status = ResetStatus.ROk
self.send_clear_auto_command()
self.send_emergency_alarm_command()
@ -1256,6 +1264,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
try:
self.feeding.run()
self.feeding.run_reseet()
except:
print(Error_Code.SYS_NONEPoint)
@ -1277,7 +1286,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
# return
log.log_message(logging.ERROR, '人员进入安全区')
time.sleep(3)
time.sleep(1)
pass
def updateUI(self):
@ -1305,12 +1314,15 @@ class MainWindow(QMainWindow, Ui_MainWindow):
self.label_date.setText(datetime.now().strftime("%Y-%m-%d"))
self.label_time.setText(datetime.now().strftime("%H:%M:%S"))
self.updateUI_Position()
self.updateUI_label_detection()
self.updateUI_label_status()
self.updateUI_frame_sign(self.feeding.feedStatus)
self.updateUI_IOPanel()
self.updateUI_InfoMB()
# self.updateUI_Position()
# self.updateUI_label_detection()
# self.updateUI_label_status()
# self.updateUI_frame_sign(self.feeding.feedStatus)
# self.updateUI_IOPanel()
# self.updateUI_InfoMB()
def updateUI_InfoMB(self):
if self.cton_take_no_photo.Q(self.feeding.take_no_photo,True):
self.show_infomessage_box("未识别到料袋报警，请重新放料后，点击继续")
@ -1533,7 +1545,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
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("StopButton")
pause_command.cmdData.append("1")
request_command = pause_command.toString()
print(request_command)