147 lines
5.4 KiB
Python
147 lines
5.4 KiB
Python
import time
|
||
|
||
# 倒料函数
|
||
# 工具坐标tool 修改为 夹具
|
||
# 倒料时的旋转角度rot_angle 默认是 180度，可以是负的, 注意限位问题
|
||
def start_drop(robot_client, rot_angle=180, tool=1, user=0, drop_vel=100):
|
||
# 1、让机器臂第六轴旋转180° (倒料)
|
||
# 获取倒料点 关节坐标，准备倒料
|
||
result = robot_client.GetActualJointPosDegree()
|
||
# result = robot.GetActualJointPosRadian()
|
||
if isinstance(result, tuple) and len(result) == 2:
|
||
# 成功：错误码和关节坐标（度数）
|
||
_, joint_pos = result
|
||
print(f"倒料点关节坐标：{joint_pos}")
|
||
else:
|
||
# 失败：result 为错误码
|
||
print(f"start_drop错误: 获取倒料点关节坐标失败，错误码：{result}")
|
||
return False
|
||
|
||
# 修改第六轴关节坐标角度，准备旋转
|
||
new_joint_pos = joint_pos.copy()
|
||
# 修改第六轴角度, 加上旋转角度
|
||
new_joint_pos[-1] = new_joint_pos[-1] + rot_angle
|
||
# print("旋转前关节坐标：", joint_pos)
|
||
# print("确认旋转后关节坐标：", new_joint_pos)
|
||
|
||
# 旋转第六轴，倒料
|
||
errno = robot_client.MoveJ(
|
||
new_joint_pos, tool=tool, user=user, vel=drop_vel
|
||
)
|
||
if errno == 0:
|
||
print(f"执行坐标: {new_joint_pos} 成功")
|
||
print("倒料成功!!!!!")
|
||
else:
|
||
print(f"start_drop错误: 旋转第六轴失败，错误码: {errno}")
|
||
return False
|
||
|
||
# 确认倒料完成
|
||
time.sleep(2)
|
||
# 这里需要抖动?
|
||
|
||
# 第六轴复位
|
||
errno = robot_client.MoveJ(joint_pos, tool=tool, user=user, vel=drop_vel)
|
||
if errno == 0:
|
||
print(f"执行坐标: {joint_pos} 成功")
|
||
print("第六轴复位成功!!!!!")
|
||
else:
|
||
print(f"start_drop错误: 第六轴复位失败，错误码: {errno}")
|
||
return False
|
||
|
||
return True # 倒料成功，包括 旋转 和 复原
|
||
|
||
# 倒料旋转函数（只旋转不复原）
|
||
# 工具坐标tool 修改为 夹具
|
||
# 倒料时的旋转角度rot_angle 默认是 180度，可以是负的, 注意限位问题
|
||
def start_drop_rotate(robot_client, rot_angle=180, tool=1, user=0, drop_vel=100, global_vel = 40):
|
||
# 0. 设置全局速度为 100%, 达到最快的倒料旋转速度
|
||
robot_client.SetSpeed(100)
|
||
|
||
# 1、让机器臂第六轴旋转180° (倒料)
|
||
# 获取倒料点 关节坐标，准备倒料
|
||
result = robot_client.GetActualJointPosDegree()
|
||
# result = robot.GetActualJointPosRadian()
|
||
if isinstance(result, tuple) and len(result) == 2:
|
||
# 成功：错误码和关节坐标（度数）
|
||
_, joint_pos = result
|
||
print(f"倒料旋转点关节坐标：{joint_pos}")
|
||
else:
|
||
# 失败：result 为错误码
|
||
print(f"start_drop_rotate错误: 获取倒料点关节坐标失败，错误码：{result}")
|
||
return False
|
||
|
||
# 修改第六轴关节坐标角度，准备旋转
|
||
new_joint_pos = joint_pos.copy()
|
||
# 修改第六轴角度, 加上旋转角度
|
||
new_joint_pos[-1] = new_joint_pos[-1] + rot_angle
|
||
# print("旋转前关节坐标：", joint_pos)
|
||
# print("确认旋转后关节坐标：", new_joint_pos)
|
||
|
||
# 旋转第六轴，倒料
|
||
errno = robot_client.MoveJ(
|
||
new_joint_pos, tool=tool, user=user, vel=drop_vel
|
||
)
|
||
|
||
# 2. 设置回原来的 全局速度
|
||
robot_client.SetSpeed(global_vel)
|
||
|
||
# 旋转倒料状态
|
||
if errno == 0:
|
||
print(f"执行坐标: {new_joint_pos} 成功")
|
||
print("倒料旋转成功!!!!!")
|
||
else:
|
||
print(f"start_drop_rotate错误: 旋转第六轴失败，错误码: {errno}")
|
||
return False
|
||
|
||
# 确认倒料完成
|
||
time.sleep(1)
|
||
# 这里需要抖动?
|
||
|
||
return True # 倒料旋转成功
|
||
|
||
# 倒料复原函数（复原, 第六轴旋转回去）
|
||
# 工具坐标tool 修改为 夹具
|
||
# 复原的旋转角度rot_angle 默认是 180度，可以是负的, 注意限位问题
|
||
def start_drop_reset(robot_client, rot_angle=180, tool=1, user=0, drop_vel=100, global_vel = 40):
|
||
# 0. 设置全局速度为 100%, 达到最快的倒料旋转速度
|
||
robot_client.SetSpeed(100)
|
||
|
||
# 1、让机器臂第六轴旋转180° (复原)
|
||
# 获取复原点 关节坐标，准备复原
|
||
result = robot_client.GetActualJointPosDegree()
|
||
# result = robot.GetActualJointPosRadian()
|
||
if isinstance(result, tuple) and len(result) == 2:
|
||
# 成功：错误码和关节坐标（度数）
|
||
_, joint_pos = result
|
||
print(f"倒料复原点关节坐标：{joint_pos}")
|
||
else:
|
||
# 失败：result 为错误码
|
||
print(f"start_drop_reset错误: 获取倒料点关节坐标失败，错误码：{result}")
|
||
return False
|
||
|
||
# 修改第六轴关节坐标角度，准备旋转
|
||
new_joint_pos = joint_pos.copy()
|
||
# 修改第六轴角度, 减去旋转角度复原
|
||
new_joint_pos[-1] = new_joint_pos[-1] - rot_angle
|
||
# print("旋转前关节坐标：", joint_pos)
|
||
# print("确认旋转后关节坐标：", new_joint_pos)
|
||
|
||
# 旋转第六轴，复原
|
||
errno = robot_client.MoveJ(
|
||
new_joint_pos, tool=tool, user=user, vel=drop_vel
|
||
)
|
||
|
||
# 2. 设置回原来的 全局速度
|
||
robot_client.SetSpeed(global_vel)
|
||
|
||
if errno == 0:
|
||
print(f"执行坐标: {new_joint_pos} 成功")
|
||
print("倒料复原成功!!!!!")
|
||
else:
|
||
print(f"start_drop_reset错误: 复原第六轴失败，错误码: {errno}")
|
||
return False
|
||
|
||
return True # 倒料复原成功
|
||
|
||
|