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)