设置舵机 ID

Bus Servo Terminal 幻尔总线舵机调试系统，用于设置每个舵机的 ID。

通信及 GUI

别问我为什么明明之前深度学习那课设还用 PyQt 现在却用 EasyGui 这样老又不再更新的东西，问就是懒……
通过查阅购买产品后提供的幻尔科技总线舵机通信协议，编写与舵机的通信命令以及图形化界面。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
import easygui
import sys
import serial
import time
##
##命令发送
##
def servoWriteCmd(id, cmd, par1 = None, par2 = None):
    buf = bytearray(b'\x55\x55')
    try:
        len = 3   #若命令是没有参数的话数据长度就是3
        buf1 = bytearray(b'')
        
        ## 对参数进行处理
        if par1 is not None:
            len += 2  #数据长度加2
            par1 = 0xffff & par1
            buf1.extend([(0xff & par1), (0xff & (par1 >> 8))])  #分低8位 高8位 放入缓存
        if par2 is not None:
            len += 2
            par2 = 0xffff & par2
            buf1.extend([(0xff & par2), (0xff & (par2 >> 8))])  #分低8位 高8位 放入缓存
    
        buf.extend([(0xff & id), (0xff & len), (0xff & cmd)]) #追加 id， 数据长度， 命令
        buf.extend(buf1) #追加参数
        
        ##计算校验和
        sum = 0x00
        for b in buf: #求和
            sum += b
        sum = sum - 0x55 - 0x55  #去掉命令开头的两个 0x55
        sum = ~sum  #取反
        buf.append(0xff & sum)  #取低8位追加进缓存
        
        serialHandle.write(buf) #发送
        
    except Exception as e:
        print(e)

##
##读取位置
##
def readPosition(id):
    #portWrite() #将单线串口配置为输出
    serialHandle.flushInput() #清空接收缓存
    servoWriteCmd(id, command["POS_READ"]) #发送读取位置命令
    time.sleep(0.00034)  #小延时，等命令发送完毕。不知道是否能进行这么精确的延时的，但是修改这个值的确实会产生影响。
                         #实验测试调到这个值的时候效果最好
    #portRead() #将单线串口配置为输入
    time.sleep(0.005)  #稍作延时，等待接收完毕
    count = serialHandle.inWaiting() #获取接收缓存中的字节数
    pos = None
    if count != 0: #如果接收到的数据不空
        recv_data = serialHandle.read(count) #读取接收到的数据
        if count == 8: #如果接收到的数据是8个字节（符合位置读取命令的返回数据的长度）
            if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == 0x1C :
                #第一第二个字节等于0x55, 第5个字节是0x1C 就是 28 就是 位置读取命令的命令号
                 pos= 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8))) #将接收到的字节数据拼接成完整的位置数据
                 #上面这小段代码我们简化了操作没有进行和校验，只要帧头和命令对了就认为数据无误
                 
    return pos  #返回读取到的位置


portx = "COM5"
bps = 115200

# 超时设置，None：永远等待操作；
#         0：立即返回请求结果；
#        其他：等待超时时间（单位为秒）
timex = None
serialHandle = serial.Serial(portx, bps, timeout=timex)
command = {"MOVE_WRITE": 1, "POS_READ": 28, "LOAD_UNLOAD_WRITE": 31}

pos = []
while True:

    msg = "舵机状态"
    title = "机械臂"
    choice = easygui.indexbox(msg, title, choices=('读取', '移动','上电','掉电'))
    duoji = [2,3,4]
    for id in duoji:
        servoWriteCmd(id, command["LOAD_UNLOAD_WRITE"], 0)  # 命令马达掉点，使舵机可以用手扭动

    if str(choice) == '0':
        pos = []
        '''
        for id in duoji:
            pos.append(readPosition(id))
            # easygui.msgbox("舵机位置为：" + str(readPosition(id)))  # readPosition(id)读取位置
            servoWriteCmd(id, command["LOAD_UNLOAD_WRITE"], 0)  # 命令马达掉电，使舵机可以用手扭动
        '''
        pos.append(readPosition(1))
        pos.append(readPosition(2))
        pos.append(readPosition(3))
        pos.append(readPosition(4))
        easygui.msgbox("舵机1状态为："+str(readPosition(1))+"\n舵机2状态为："+str(readPosition(2))+"\n舵机3状态为："+str(readPosition(3))+"\n舵机4状态为："+str(readPosition(4)),"结果")  #readPosition(id)读取位置
    elif str(choice) == '1':
        for id_index in range(len(duoji)):
            id = duoji[id_index]
            servoWriteCmd(id, command["MOVE_WRITE"], pos[id_index], 200)  # 移动的代码
        easygui.msgbox("正在移动")
    elif str(choice)=='2':
        for id in duoji:
            servoWriteCmd(id, command["LOAD_UNLOAD_WRITE"], 1) #发送写入上电命令
    elif str(choice)=='3':
        for id in duoji:
            servoWriteCmd(id, command["LOAD_UNLOAD_WRITE"], 0) #发送写入掉电命令

    msg = "你希望重新读取状态？"
    title = "请选择"
    if easygui.ccbox(msg, title):
        pass
    else:
        sys.exit(0)

参考资料

EasyGUI Tutorial