-
Notifications
You must be signed in to change notification settings - Fork 0
/
packet.py
79 lines (66 loc) · 1.75 KB
/
packet.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
""" Serial communication primitives """
import array
# PRINT_PACKETS = 1 # uncomment to print packets sent and received
def makePacket( id, instruction, params ):
""" Make a dynamixel formatted packet """
p = [
0xff,
0xff,
id & 0xff,
len(params)+2,
instruction & 0xff
]
for param in params:
p.append(param & 0xff)
p.append(checksumPacket(p))
return p
def checksumPacket( p ):
""" Calculate the checksum byte for the packet """
sum = 0
for byte in p[2:]:
sum = 0xff & (sum + byte)
notSum = 0xff & (~sum)
return notSum
def checkPacket( id, p ):
"""
check bytes for errors or unexpected conditions
(http://support.robotis.com/en/product/dynamixel/communication/dxl_packet.htm)
"""
if p[2] != id:
print 'Bad packet read (Unexpected id)'
return -1
if p[3] + 4 != len(p):
print 'Bad packet read (Incorrect length)'
return -1
if p[4] != 0x00:
print 'Bad packet read (Error bits set: ', p[4], ' [decimal representation])'
return -1
if p[-1] != checksumPacket(p[:-1]):
print 'Bad packet read (bad checksum)'
return -1
return 0
def p2str( p ):
""" Convert packet to string """
return array.array('B', p).tostring();
def str2p( s ):
""" Convert string to packet """
return [ord(char) for char in list(s)]
def sendPacket( ser, p ):
""" Send packet over serial channel """
if PRINT_PACKETS:
print 'sent: ', p
i = ser.write(p2str(p))
if i == 0:
print 'No bytes written in sendPacket'
return
def receivePacket( ser, id ):
""" Read a packet waiting in the buffer """
strHead = ser.read(4) # read packet up to length byte
pHead = str2p(strHead)
strTail = ser.read(pHead[3]) # read remaining bytes
p = str2p(strHead + strTail)
if checkPacket(id, p) != 0:
return None
if PRINT_PACKETS:
print 'received: ', p
return p