机械臂设备检测

一、API 简介

1、is_all_servo_enable()

函数功能：判断六个关节点是否可以正常使用。

返回参数：1 表示正常工作，0 表示不能工作，-1 表示错误报警。

2、jog_angle(joint_id,direction,speed)

函数功能：让一个关节点持续移动。

参数说明：

joint_id：取值范围 1~6，六个整数分别表示关节点 1~6。

direction：1 表示角度增加，0 表示角度减少。

speed：表示增加减少的速度。

3、jog_stop()

函数功能：停止关节点的移动。

4、release_servo(servo_id)

函数功能：放松指定关节点。

参数说明：

servo_id：取值范围 1~6，六个整数分别表示关节点 1~6。

5、focus_servo(servo_id)

函数功能：固定指定关节点。

参数说明：

servo_id：取值范围 1~6，六个整数分别表示关节点 1~6。

二、代码内容

from pymycobot.mycobot import MyCobot
from pymycobot import PI_PORT, PI_BAUD  # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
import time

# MyCobot 类初始化需要两个参数：
#   第一个是串口字符串， 如：
#       linux： "/dev/ttyUSB0"
#       windows: "COM3"
#   第二个是波特率：
#       M5版本为： 115200
#
#   Example:
#       mycobot-M5:
#           linux:
#              mc = MyCobot("/dev/ttyUSB0", 115200)
#           windows:
#              mc = MyCobot("COM3", 115200)
#       mycobot-raspi:
#           mc = MyCobot(PI_PORT, PI_BAUD)
#
# 初始化一个MyCobot对象
# 下面为树莓派版本创建对象代码
mc = MyCobot(PI_PORT, PI_BAUD)
# 判断机械臂是否供电，若无供电需要先为其供电
if not mc.is_power_on():
    # 为机械臂供电
    mc.power_on()

# 检测六个关节是否正常工作
# 也可以使用is_servo_enable(servo_id)改变单个单个校验
if mc.is_all_servo_enable():
    # 为机械臂断电
    mc.power_off()
    # 判断机械臂是否断电
    if mc.is_all_servo_enable() == -1:
        print("机械臂供电断电正常")
    else:
        print("机械臂供电断电失败")
        exit(0)

# 为机械臂重新充电
mc.power_on()
# 将机械臂设置到零位上
mc.send_angles([0, 0, 0, 0, 0, 0], 30)
# 获取当前时间
start = time.time()
# 判断机械臂是否到达零点
while not mc.is_in_position([0, 0, 0, 0, 0, 0], 0):
    # 让机械臂恢复运动
    mc.resume()
    # 让机械臂移动0.5s
    time.sleep(0.5)
    # 暂停机械臂移动
    mc.pause()
    # 判断移动是否超时
    if time.time() - start > 9:
        # 停止机械臂的移动
        print("移动到零位失败")
        # 中止程序
        exit(0)

# 检测六个关节点的移动情况
for i in range(1, 7):
    # 让关节点i向右以速度15移动
    mc.jog_angle(i, 0, 15)
    # 让关节点移动1.5s
    time.sleep(1.5)
    # 停止关节点移动
    mc.jog_stop()
    # 让关节点i向左以速度15移动
    mc.jog_angle(i, 1, 15)
    # 让关节点移动3s
    time.sleep(3)
    # 停止关节点移动
    mc.jog_stop()
    # 让关节点i向右以速度15移动
    mc.jog_angle(i, 0, 15)
    # 让关节点移动1.5s
    time.sleep(1.5)
    # 停止关节点移动
    mc.jog_stop()
    print(str(i) + "号关节点正常工作")
    # 等待0.8s
    time.sleep(0.8)

# 获取当前时间
start = time.time()
# 机械臂以30的速度到达[87.27, -139.13, 153.72, -160.92, -74.44, 7.55]位置
mc.send_angles([87.27, -139.13, 153.72, -160.92, -74.44, 7.55], 30)
# 判断机械臂是否到达[87.27, -139.13, 153.72, -160.92, -74.44, 7.55]位置
while not mc.is_in_position([87.27, -139.13, 153.72, -160.92, -74.44, 7.55], 0):
    # 恢复机械臂的移动
    mc.resume()
    # 让机械臂移动0.5s
    time.sleep(0.5)
    # 暂停机械臂移动
    mc.pause()
    # 判断移动是否超时
    if time.time() - start > 9:
        mc.stop()
        # 停止机械臂的移动
        break

# 该for循环相当于set_free_model()
for i in range(1, 7):
    mc.release_servo(i)

# 依次固定六个关节点
# for i in range(1, 7):
#     mc.focus_servo(i)

三、效果展示