幻尔串口总线舵机控制Python SDK

一、材料准备

幻尔总线舵机（我用的是xArm机械臂）

幻尔TTL/USB调试板（一定要是幻尔的，其他牌子的调试板驱动不了舵机）

二、Python SDK

"""
串口舵机Python SDK
----------------------------------------------
"""
import serial
import time
import ctypes

###################################################################################################
# 幻尔科技总线舵机通信协议
###################################################################################################
ROBOT_SERVO_FRAME_HEADER = 0x55
ROBOT_SERVO_MOVE_TIME_WRITE = 1  # 该命令发送给舵机，舵机将在参数时间内从当前角度匀速转动到参数角度
ROBOT_SERVO_MOVE_TIME_READ = 2  # 读取发送给舵机的角度运动指令
ROBOT_SERVO_MOVE_TIME_WAIT_WRITE = 7  # 舵机转动指令，等待一段时间后转动
ROBOT_SERVO_MOVE_TIME_WAIT_READ = 8  # 读取上面的指令
ROBOT_SERVO_MOVE_START = 11  # 舵机转动指令，舵机立即开始转动
ROBOT_SERVO_MOVE_STOP = 12  # 舵机停止指令
ROBOT_SERVO_ID_WRITE = 13  # 重新写入舵机ID
ROBOT_SERVO_ID_READ = 14  # 读取舵机的ID值
ROBOT_SERVO_ANGLE_OFFSET_ADJUST = 17  # 舵机转动角度偏差调整
ROBOT_SERVO_ANGLE_OFFSET_WRITE = 18  # 保存偏差值
ROBOT_SERVO_ANGLE_OFFSET_READ = 19  # 读取偏差值
ROBOT_SERVO_ANGLE_LIMIT_WRITE = 20  # 设置舵机转动的范围
ROBOT_SERVO_ANGLE_LIMIT_READ = 21  # 读取舵机转动的范围
ROBOT_SERVO_VIN_LIMIT_WRITE = 22  # 设置舵机的电压范围
ROBOT_SERVO_VIN_LIMIT_READ = 23  # 读取舵机的电压范围
ROBOT_SERVO_TEMP_MAX_LIMIT_WRITE = 24  # 设置舵机内部最高温度
ROBOT_SERVO_TEMP_MAX_LIMIT_READ = 25  # 读取舵机允许最高温度
ROBOT_SERVO_TEMP_READ = 26  # 读取舵机实时温度
ROBOT_SERVO_VIN_READ = 27  # 读取舵机实时电压
ROBOT_SERVO_POS_READ = 28  # 读取舵机当前实际角度位置值
ROBOT_SERVO_OR_MOTOR_MODE_WRITE = 29  # 控制舵机的转速
ROBOT_SERVO_OR_MOTOR_MODE_READ = 30  # 读取舵机控制模式的相关参数
ROBOT_SERVO_LOAD_OR_UNLOAD_WRITE = 31  # 设置舵机内部电机是否掉电
ROBOT_SERVO_LOAD_OR_UNLOAD_READ = 32  # 读取舵机内部电机状态
ROBOT_SERVO_LED_CTRL_WRITE = 33  # 设置LED灯状态
ROBOT_SERVO_LED_CTRL_READ = 34  # 读取LED灯状态
ROBOT_SERVO_LED_ERROR_WRITE = 35  # 设置导致LED灯闪烁报警的值
ROBOT_SERVO_LED_ERROR_READ = 36  # 读取舵机故障报警值

serialHandle = serial.Serial("COM5", 115200)  # 初始化串口， 波特率为115200
time_out = 50


###################################################################################################
# 舵机与PC通信
###################################################################################################
def checksum(buf):
    """计算校验和"""
    check_sum = 0x00
    for b in buf:
        check_sum += b
    check_sum = check_sum - 0x55 - 0x55
    check_sum = ~check_sum  # 取反
    return check_sum & 0xff


def serial_servo_write(ID=None, w_cmd=None, dat1=None, dat2=None):
    """写指令"""
    buf = bytearray(b'\x55\x55')  # 帧头
    buf.append(ID)
    # 指令长度
    if dat1 is None and dat2 is None:
        buf.append(3)
    elif dat1 is not None and dat2 is None:
        buf.append(4)
    elif dat1 is not None and dat2 is not None:
        buf.append(7)

    buf.append(w_cmd)  # 指令
    # 写数据
    if dat1 is None and dat2 is None:
        pass
    elif dat1 is not None and dat2 is None:
        buf.append(dat1 & 0xff)
    elif dat1 is not None and dat2 is not None:
        buf.extend([(0xff & dat1), (0xff & (dat1 >> 8))])  # 分低8位 高8位 放入缓存
        buf.extend([(0xff & dat2), (0xff & (dat2 >> 8))])  # 分低8位 高8位 放入缓存
    # 校验和
    buf.append(checksum(buf))

    serialHandle.write(buf)  # 发送


def serial_servo_read(ID=None, r_cmd=None):
    """发送读取命令"""
    buf = bytearray(b'\x55\x55')  # 帧头
    buf.append(ID)
    buf.append(3)  # 指令长度
    buf.append(r_cmd)  # 指令
    buf.append(checksum(buf))  # 校验和
    serialHandle.write(buf)  # 发送
    time.sleep(0.00034)


def serial_servo_get_r_msg(cmd):
    """获取指定读取数据命令的数据"""
    serialHandle.flushInput()  # 清空接收缓存
    time.sleep(0.005)  # 稍作延迟，等待接收完毕
    count = serialHandle.inWaiting()  # 获取接收缓存中的字节数
    if count != 0:
        recv_data = serialHandle.read(count)  # 读取接收到的数据
        try:
            if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == cmd:
                dat_len = recv_data[3]
                serialHandle.flushInput()  # 清空接收缓存
                if dat_len == 4:
                    return recv_data[5]
                elif dat_len == 5:
                    pos = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8)))
                    return ctypes.c_int16(pos).value
                elif dat_len == 7:
                    pos1 = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8)))
                    pos2 = 0xffff & (recv_data[7] | (0xff00 & (recv_data[8] << 8)))
                    return ctypes.c_int16(pos1).value, ctypes.c_int16(pos2).value
            else:
                return None
        except BaseException as e:
            print(e)
    else:
        serialHandle.flushInput()  # 清空接收缓存
        return None


###################################################################################################
# 舵机使用API
###################################################################################################
def setBusServoID(old_id, new_id):
    """配置舵机ID号，出厂默认为1"""
    serial_servo_write(old_id, ROBOT_SERVO_ID_WRITE, new_id)


def getBusServoID(ID=None):
    """读取串口舵机ID"""
    while True:
        if ID is None:  # 总线上只能有一个舵机
            serial_servo_read(0xfe, ROBOT_SERVO_ID_READ)
        else:
            serial_servo_read(ID, ROBOT_SERVO_ID_READ)
        # 获取内容
        msg = serial_servo_get_r_msg(ROBOT_SERVO_ID_READ)
        if msg is not None:
            return msg


def setBusServoPulse(ID, pulse, use_time):
    """驱动串口舵机运动到指定位置"""
    pulse = 0 if pulse < 0 else pulse
    pulse = 1000 if pulse > 1000 else pulse
    use_time = 0 if use_time < 0 else use_time
    use_time = 30000 if use_time > 30000 else use_time
    serial_servo_write(ID, ROBOT_SERVO_MOVE_TIME_WRITE, pulse, use_time)


def getBusServoPulse(ID):
    """
    读取舵机当前位置
    """
    while True:
        serial_servo_read(ID, ROBOT_SERVO_POS_READ)
        msg = serial_servo_get_r_msg(ROBOT_SERVO_POS_READ)
        if msg is not None:
            return msg


def stopBusServo(ID=None):
    """停止舵机运行"""
    serial_servo_write(ID, ROBOT_SERVO_MOVE_STOP)


def setBusServoDeviation(ID, d=0):
    """调整偏差"""
    serial_servo_write(ID, ROBOT_SERVO_ANGLE_OFFSET_ADJUST, d)


def saveBusServoDeviation(ID):
    """配置偏差，掉电保护"""
    serial_servo_write(ID, ROBOT_SERVO_ANGLE_OFFSET_WRITE)


def getBusServoDeviation(ID):
    """读取偏差值"""
    # 发送读取偏差指令
    count = 0
    while True:
        serial_servo_read(ID, ROBOT_SERVO_ANGLE_OFFSET_READ)
        # 获取
        msg = serial_servo_get_r_msg(ROBOT_SERVO_ANGLE_OFFSET_READ)
        count += 1
        if msg is not None:
            return msg
        if count > time_out:
            return None


def setBusServoAngleLimit(ID, low, high):
    """设置舵机转动范围"""
    serial_servo_write(ID, ROBOT_SERVO_ANGLE_LIMIT_WRITE, low, high)


def getBusServoAngleLimit(ID):
    """读取舵机转动范围"""
    while True:
        serial_servo_read(ID, ROBOT_SERVO_ANGLE_LIMIT_READ)
        msg = serial_servo_get_r_msg(ROBOT_SERVO_ANGLE_LIMIT_READ)
        if msg is not None:
            return msg


def setBusServoVinLimit(ID, low, high):
    """设置舵机电压范围"""
    serial_servo_write(ID, ROBOT_SERVO_VIN_LIMIT_WRITE, low, high)


def getBusServoVinLimit(ID):
    """读取舵机转动范围"""
    while True:
        serial_servo_read(ID, ROBOT_SERVO_VIN_LIMIT_READ)
        msg = serial_servo_get_r_msg(ROBOT_SERVO_VIN_LIMIT_READ)
        if msg is not None:
            return msg


def setBusServoMaxTemp(ID, m_temp):
    """设置舵机最高温度报警"""
    serial_servo_write(ID, ROBOT_SERVO_TEMP_MAX_LIMIT_WRITE, m_temp)


def getBusServoTempLimit(ID):
    """读取舵机温度报警范围"""
    while True:
        serial_servo_read(ID, ROBOT_SERVO_TEMP_MAX_LIMIT_READ)
        msg = serial_servo_get_r_msg(ROBOT_SERVO_TEMP_MAX_LIMIT_READ)
        if msg is not None:
            return msg


def getBusServoTemp(ID):
    """读取舵机温度"""
    while True:
        serial_servo_read(ID, ROBOT_SERVO_TEMP_READ)
        msg = serial_servo_get_r_msg(ROBOT_SERVO_TEMP_READ)
        if msg is not None:
            return msg


def getBusServoVin(ID):
    """读取舵机电压"""
    while True:
        serial_servo_read(ID, ROBOT_SERVO_VIN_READ)
        msg = serial_servo_get_r_msg(ROBOT_SERVO_VIN_READ)
        if msg is not None:
            return msg


def restBusServoPulse(old_id):
    # 舵机清零偏差和P值中位（500）
    setBusServoDeviation(old_id, 0)    # 清零偏差
    time.sleep(0.1)
    serial_servo_write(old_id, ROBOT_SERVO_MOVE_TIME_WRITE, 500, 100)    # 中位


def unloadBusServo(ID):
    serial_servo_write(ID, ROBOT_SERVO_LOAD_OR_UNLOAD_WRITE, 0)


def getBusServoLoadStatus(ID):
    """读取是否掉电"""
    while True:
        serial_servo_read(ID, ROBOT_SERVO_LOAD_OR_UNLOAD_READ)
        msg = serial_servo_get_r_msg(ROBOT_SERVO_LOAD_OR_UNLOAD_READ)
        if msg is not None:
            return msg

 三、调用说明

# 导入API
from servo import *



# 驱动舵机运动
setBusServoPulse(1, 500, 500)