python——德晟串口总线智能舵机驱动

花了一晚上+一早上完成总线舵机的大部分常用指令编写，分享给大家，只要安装pyserial即可使用~

总线舵机

我买的是某宝上亚博的智能总线舵机，用来做机械臂。

舵机优势

有位置反馈，这对做机器人、机械臂十分有帮助，可以进行运动学解算。

驱动板

总线协议

驱动库

''' 
@Author: Irving.Gao  
@Date: 2021-05-02 22:02:20  
@Last Modified by:   Irving.Gao  
@Last Modified time: 2021-05-02 22:02:20  
'''
# -*- coding:UTF-8 -*-
import time
import serial

class Serservo:
    '''
    @串行舵机控制驱动库

    @方法：
        设置ID：  serial_servo_set_ID(id)
        查询舵机：serial_servo_ping(id)
        读取舵机：serial_servo_read(id)
        写舵机：  serial_servo_write(id, pos, time)
        同步控制：serial_servo_sync_write(id_list, pos_list, time_list)
        异步控制：serial_servo_reg_write(id_list, pos_list, time_list)
        关闭通信：serial_servo_close()
    '''
    def __init__(self, port):
        self.ser = serial.Serial(port, 115200, timeout=0.001)
        # 通信协议
        # data = [head_1, head_2, id, len, cmd, addr, pos1, pos2, time1, time2, checknum]
        # 通信协议数据常量
        self.head_1 = 0xFF
        self.head_2 = 0xFF
        self.all_id = 0xFE

        # 数据长度
        self.len_ping = 0x02            # 查询指令长度
        self.len_write = 0x07           # 写指令长度
        self.len_read = 0x04            # 读指令长度
        self.len_action = 0x02          # 执行异步写指令长度

        # 控制指令常量
        self.cmd_ping = 0x01            # 查询舵机 
        self.cmd_read_data = 0x02       # 查询指定地址的数据
        self.cmd_write_data = 0x03      # 向指定地址写数据
        self.cmd_reg_write = 0x04       # 向指定地址预写数据，等收到 向指定地址预写数据，等收到 ACTIONACTION ACTION ACTION指令后才执行，主要用于控制多个舵机 指令后才执行，主要用于控制多个舵机 时能让舵机同启动
        self.cmd_action = 0x05          # 触发执行REG WRITE指令
        self.cmd_reset = 0x06           # 把寄存器恢复为出厂设定值
        self.cmd_sync_write = 0x83      # 用于同时向多个舵机写入不参数

        # 控制指令对应地址
        self.addr_id = 0x05             # 舵机ID
        self.addr_write = 0x2A        # 目标位置 0-4095
        self.addr_write_L = 0x2A        # 目标位置 0-4095
        self.addr_write_H = 0x2B        # 目标位置 0-4095
        self.addr_runtime_L = 0x2C      # 运行时间 ms
        self.addr_runtime_H = 0x2D      # 运行时间 ms
        
        self.addr_read = 0x38         # 当前位置 0-4095
        self.addr_read_L = 0x38         # 当前位置 0-4095
        self.addr_read_H = 0x39         # 当前位置 0-4095
        
        # 控制指令写入的参数
        self.param_read = 0x02         # 当前位置 0-4095

    def serial_servo_ping(self, id):
        '''
        @函数功能：检查对应id的舵机是否存在
        @输入参数：
            id: 1-250 舵机ID
        '''
        checknum = (~(id + self.len_ping + self.cmd_ping)) & 0xff

        data = [self.head_1, 
                self.head_2, 
                id, 
                self.len_ping, 
                self.cmd_ping, 
                checknum]
        
        self.ser.write(bytes(data)) # 发送读取指令
        time.sleep(0.001)
        rev_data = self.ser.readline()  # 读取舵机返回指令
        rev_data = rev_data.hex()
        if rev_data != "":
            # 示例：fff5030200fa >——> 参数：0200
            ping_data = str(rev_data[8:10]) # 提取出返回参数
            ping_status = int(ping_data, 10) # 将字符串转换成10进制
            if ping_status == 0:
                print("the {} servo is available".format(id))
        else:
            print("the {} servo is not detected".format(id))

    def serial_servo_read(self, id):
        '''
        @函数功能：读取总线舵机位置。
        
        @输入参数：
            id: 1-250 舵机ID
        @返回参数：
            pos: 0-4095 舵机的位置
        '''
        checknum = (~(id + self.len_read + self.cmd_read_data + self.addr_read + self.param_read)) & 0xff

        data = [self.head_1, 
                self.head_2, 
                id, 
                self.len_read, 
                self.cmd_read_data, 
                self.addr_read, 
                self.param_read, 
                checknum]
        
        self.ser.write(bytes(data)) # 发送读取指令
        time.sleep(0.001)
        rev_data = self.ser.readline()  # 读取舵机返回指令
        rev_data = rev_data.hex()
        if rev_data != "":
            # 示例：fff50104000c1cd2 >——> 参数：0c1c
            pos = str(rev_data[10:14]) # 提取出返回参数
            pos = int(pos, 16) # 将字符串转换成16进制
        else:
            pos = None
        
        return pos

    def serial_servo_write(self, id, pos, tim):
        '''
        @函数功能：设置总线舵机角度接口。
        @输入参数：
            id: 1-250 舵机ID
            pos: 0-4095 舵机的位置
            time: 运动时间
        '''
        pos_H = (pos >> 8) & 0xFF
        pos_L = pos & 0xFF
        time_H = (tim >> 8) & 0xFF
        time_L = tim & 0xFF

        checknum = (~(id + self.len_write + self.cmd_write_data + self.addr_write + pos_H + pos_L + time_H + time_L)) & 0xff
        data = [self.head_1, 
                self.head_2, 
                id, 
                self.len_write, 
                self.cmd_write_data, 
                self.addr_write, 
                pos_H, 
                pos_L, 
                time_H, 
                time_L, 
                checknum]

        self.ser.write(bytes(data))
        time.sleep(tim/1000) # 延时让机械臂完成动作

    def serial_servo_reg_write(self, id_list, pos_list, time_list):
        '''
        @函数功能：异步设置总线舵机角度接口。
        @输入参数：
            id_list: [id1,id2,...] 1-250 舵机ID
            pos_list: [pos1,pos2,...] 0-4095 舵机的位置
            time_list: [time1,time2,...] 运动时间
        '''
        # data_list = []
        print(id_list)
        if len(id_list) != len(pos_list) or len(id_list) != len(time_list) or len(pos_list) != len(time_list):
            print("params are incorroct!")
            return None

        for i in range(len(id_list)):
            # print(i)
            # i += 1
            id = id_list[i]
            pos_H = (pos_list[i] >> 8) & 0xFF
            pos_L = pos_list[i] & 0xFF
            time_H = (time_list[i] >> 8) & 0xFF
            time_L = time_list[i] & 0xFF
            # data_list.append([pos_H, pos_L, time_H, time_L])
        
            reg_checknum = (~(id + self.len_write + self.cmd_reg_write + self.addr_write + pos_H + pos_L + time_H + time_L)) & 0xff
            reg_data = [self.head_1, 
                        self.head_2, 
                        id, 
                        self.len_write, 
                        self.cmd_reg_write, 
                        self.addr_write, 
                        pos_H, 
                        pos_L, 
                        time_H, 
                        time_L, 
                        reg_checknum]
            self.ser.write(bytes(reg_data))
            time.sleep(0.01)

        act_checknum = (~(self.all_id + self.len_action + self.cmd_action)) & 0xff
        action_data = [self.head_1, 
                       self.head_2, 
                       self.all_id, 
                       self.len_action, 
                       self.cmd_action, 
                       act_checknum]
        self.ser.write(bytes(action_data))
        time.sleep(max(time_list)/1000) # 延时让机械臂完成动作
        
    def serial_servo_sync_write(self, id_list, pos_list, time_list):
        '''
        @函数功能：同步设置多个总线舵机位置。
        @输入参数：
            id_list: [id1,id2,...] 1-250 舵机ID
            pos_list: [pos1,pos2,...] 0-4095 舵机的位置
            time_list: [time1,time2,...] 运动时间
        '''
        if len(id_list) != len(pos_list) or len(id_list) != len(time_list) or len(pos_list) != len(time_list):
            print("params are incorroct!")
            return None

        sync_params = []
        sum_params = 0
        for i in range(len(id_list)):
            # print(i)
            # i += 1
            id = id_list[i]
            pos_H = (pos_list[i] >> 8) & 0xFF
            pos_L = pos_list[i] & 0xFF
            time_H = (time_list[i] >> 8) & 0xFF
            time_L = time_list[i] & 0xFF
            sync_params += [id, pos_H, pos_L, time_H, time_L]
        sum_params = sum(sync_params) # 写入的参数求和

        len_data = len(sync_params) + 2 + 2         # 数据长度 N(参数)+2
        len_servo_params = len(id_list) * 2     # 要给舵机写入的数据长度
        sync_checknum = (~(self.all_id + len_data + self.cmd_sync_write + self.addr_write + len_servo_params + sum_params)) & 0xff
        sync_data = [self.head_1, 
                    self.head_2, 
                    self.all_id, 
                    len_data, 
                    self.cmd_sync_write, 
                    self.addr_write,
                    len_servo_params] + \
                    sync_params + \
                    [sync_checknum]

        self.ser.write(bytes(sync_data))
        time.sleep(max(time_list)/1000) # 延时让机械臂完成动作

    def serial_servo_set_ID(self, id):
        '''
        @函数功能：设置单个舵机的id编号，使用时务必确保只连接了单个舵机。
        
        @输入参数：
            id： 1-250 舵机ID

        '''
        checknum = (~(self.all_id + self.len_write + self.cmd_write_data + self.addr_id + id)) & 0xff
        data = [self.head_1, 
                self.head_2, 
                self.all_id, 
                self.len_write, 
                self.cmd_write_data, 
                self.addr_id, 
                id, 
                checknum]

        self.ser.write(bytes(data))

    
    def serial_servo_close(self):
        '''
        @函数功能：关闭机械臂串行通信
        '''
        self.ser.close()