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

修改跟随法向量方向移动问题

Browse Source
This commit is contained in:
cdeyw
2025-03-04 15:52:54 +08:00
parent a9cce7ec3a
commit 69e88c2b6b
9 changed files with 7230 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
CU/Detect.py
View File

@ -39,7 +39,10 @@ class Detect:
position = Real_Position().init_position(*target_position[:3], *noraml_base[:3])
position.Z = position.Z
# position.Z = position.Z
position.a = uvw[0]
position.b = uvw[1]
position.c = uvw[2]
self.detect_position = position
self.detect_status = DetectStatus.DOk

4
CU/EMV.py
View File

@ -36,7 +36,7 @@ def send_command(command):
print(f"收到响应: {binascii.hexlify(response)}")
# 校验响应
if response == byte_data:
print("命令成功下发，继电器已执行操作。")
# print("命令成功下发，继电器已执行操作。")
return True
else:
print("命令下发失败，响应与请求不符。")
@ -67,7 +67,7 @@ def close(grasp, shake, throw):
if send_command(valve_commands[1]['close']):
time.sleep(1)
if shake:
print("关闭电磁阀 2")
# print("关闭电磁阀 2")
if send_command(valve_commands[2]['close']):
time.sleep(0.05)
if throw:

31
CU/Feeding.py
View File

@ -20,6 +20,7 @@ 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 Trace.handeye_calibration import getxyz
from Util.util_math import get_distance
from Util.util_time import CRisOrFall
from Vision.camera_coordinate_dete import Detection
@ -132,21 +133,21 @@ class FeedLine:
def set_take_position(self,position:Real_Position,dynamic_height=0):
for i in range(len(self.feeding_to_end)):
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
if position != None:
befor_take_position = Real_Position().init_position(position.X,
position.Y,
position.Z+dynamic_height,
position.U,
position.V,
position.W)
after_take_position = Real_Position().init_position(position.X,
position.Y,
position.Z+dynamic_height,
position.U,
position.V,
position.W)
self.feeding_to_end[i - 1].set_position(befor_take_position)
self.feeding_to_end[i + 1].set_position(after_take_position)
xyz = getxyz(position.X, position.Y, position.Z, position.a, position.b, position.c)
befor_take_position = Real_Position().init_position(xyz[0],
xyz[1],
xyz[2],
position.U,
position.V,
position.W)
after_take_position = Real_Position().init_position(xyz[0],
xyz[1],
xyz[2],
position.U,
position.V,
position.W)
self.feeding_to_end[i - 1].set_position(befor_take_position)
self.feeding_to_end[i + 1].set_position(after_take_position)
self.feeding_to_end[i].set_position(position)