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: