-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathBusServoCmd.py
167 lines (153 loc) · 5.25 KB
/
BusServoCmd.py
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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
#!/usr/bin/env python3
# encoding: utf-8
import time
import serial
import ctypes
import RPi.GPIO as GPIO
#幻尔科技总线舵机通信#
LOBOT_SERVO_FRAME_HEADER = 0x55
LOBOT_SERVO_MOVE_TIME_WRITE = 1
LOBOT_SERVO_MOVE_TIME_READ = 2
LOBOT_SERVO_MOVE_TIME_WAIT_WRITE = 7
LOBOT_SERVO_MOVE_TIME_WAIT_READ = 8
LOBOT_SERVO_MOVE_START = 11
LOBOT_SERVO_MOVE_STOP = 12
LOBOT_SERVO_ID_WRITE = 13
LOBOT_SERVO_ID_READ = 14
LOBOT_SERVO_ANGLE_OFFSET_ADJUST = 17
LOBOT_SERVO_ANGLE_OFFSET_WRITE = 18
LOBOT_SERVO_ANGLE_OFFSET_READ = 19
LOBOT_SERVO_ANGLE_LIMIT_WRITE = 20
LOBOT_SERVO_ANGLE_LIMIT_READ = 21
LOBOT_SERVO_VIN_LIMIT_WRITE = 22
LOBOT_SERVO_VIN_LIMIT_READ = 23
LOBOT_SERVO_TEMP_MAX_LIMIT_WRITE = 24
LOBOT_SERVO_TEMP_MAX_LIMIT_READ = 25
LOBOT_SERVO_TEMP_READ = 26
LOBOT_SERVO_VIN_READ = 27
LOBOT_SERVO_POS_READ = 28
LOBOT_SERVO_OR_MOTOR_MODE_WRITE = 29
LOBOT_SERVO_OR_MOTOR_MODE_READ = 30
LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE = 31
LOBOT_SERVO_LOAD_OR_UNLOAD_READ = 32
LOBOT_SERVO_LED_CTRL_WRITE = 33
LOBOT_SERVO_LED_CTRL_READ = 34
LOBOT_SERVO_LED_ERROR_WRITE = 35
LOBOT_SERVO_LED_ERROR_READ = 36
rx_pin = 11
tx_pin = 13
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
serialHandle = serial.Serial("/dev/ttyAMA0", 115200) # 初始化串口， 波特率为115200
def portInit(): # 配置用到的IO口
GPIO.setup(rx_pin, GPIO.OUT) # 配置RX_CON 即 GPIO17 为输出
GPIO.output(rx_pin, 0)
GPIO.setup(tx_pin, GPIO.OUT) # 配置TX_CON 即 GPIO27 为输出
GPIO.output(tx_pin, 1)
portInit()
def portWrite(): # 配置单线串口为输出
GPIO.output(tx_pin, 1) # 拉高TX_CON 即 GPIO27
GPIO.output(rx_pin, 0) # 拉低RX_CON 即 GPIO17
def portRead(): # 配置单线串口为输入
GPIO.output(rx_pin, 1) # 拉高RX_CON 即 GPIO17
GPIO.output(tx_pin, 0) # 拉低TX_CON 即 GPIO27
def portRest():
time.sleep(0.1)
serialHandle.close()
GPIO.output(rx_pin, 1)
GPIO.output(tx_pin, 1)
serialHandle.open()
time.sleep(0.1)
def checksum(buf):
# 计算校验和
sum = 0x00
for b in buf: # 求和
sum += b
sum = sum - 0x55 - 0x55 # 去掉命令开头的两个 0x55
sum = ~sum # 取反
return sum & 0xff
def serial_serro_wirte_cmd(id=None, w_cmd=None, dat1=None, dat2=None):
'''
写指令
:param id:
:param w_cmd:
:param dat1:
:param dat2:
:return:
'''
portWrite()
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))
# for i in buf:
# print('%x' %i)
serialHandle.write(buf) # 发送
def serial_servo_read_cmd(id=None, r_cmd=None):
'''
发送读取命令
:param id:
:param r_cmd:
:param dat:
:return:
'''
portWrite()
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_rmsg(cmd):
'''
# 获取指定读取命令的数据
:param cmd: 读取命令
:return: 数据
'''
serialHandle.flushInput() # 清空接收缓存
portRead() # 将单线串口配置为输入
time.sleep(0.005) # 稍作延时，等待接收完毕
count = serialHandle.inWaiting() # 获取接收缓存中的字节数
if count != 0: # 如果接收到的数据不空
recv_data = serialHandle.read(count) # 读取接收到的数据
# for i in recv_data:
# print('%#x' %ord(i))
# 是否是读id指令
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:
# print ctypes.c_int8(ord(recv_data[5])).value # 转换成有符号整型
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