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
|
RD_VERSION = 0x00
WRITE_INFO = 0x01
READ_INFO = 0x02
SEND_RAW = 0x03
def getVersion(dev):
dev.send([RD_VERSION])
raw = dev.read(3)
return raw[1] + raw[2] * 256
def writeInfo(dev, motor_id, regstart, value):
msg = [WRITE_INFO, motor_id, regstart, value / 256, value % 256]
dev.send(msg)
raw = dev.read(2)
return raw[1]
def readInfo(dev, motor_id, regstart, lenght):
msg = [READ_INFO, motor_id, regstart, lenght]
dev.send(msg)
raw = dev.read(3)
if lenght == 1:
return raw[1]
else:
return raw[1] + raw[2] * 256
def sendPacket(dev, pack):
wait_resp = len(pack) + 2
msg = [SEND_RAW, wait_resp] + pack
dev.send(msg)
raw = dev.read(255)
if len(raw) == 1:
return -1 # only opcode o nil
timeout = raw[2]
print 'timeout', timeout
if timeout == 1:
return -1
size = raw[1]
print "AX12 answer\n:::SIZE = " + str(size) + "\n:::TIMEOUT = " + str(timeout)
msg = ''
for i in range(3,size+3):
msg = msg + str(raw[i]) + ' '
print ":::MESSAGE\n " + msg
return msg
def wheelMode(dev, motor_id):
msg = [WRITE_INFO, motor_id, 0x06, 0x00, 0x00]
dev.send(msg)
raw = dev.read(2)
msg = [WRITE_INFO, motor_id, 0x08, 0x00, 0x00]
dev.send(msg)
raw = dev.read(2)
return raw[1]
def jointMode(dev, motor_id, _min, _max):
msg = [WRITE_INFO, motor_id, 0x06, _min / 256, _min % 256]
dev.send(msg)
raw = dev.read(2)
msg = [WRITE_INFO, motor_id, 0x08, _max / 256, _max % 256]
dev.send(msg)
raw = dev.read(2)
return raw[1]
def setPosition(dev, motor_id, pos):
msg = [WRITE_INFO, motor_id, 0x1E, pos / 256, pos % 256]
dev.send(msg)
raw = dev.read(2)
return raw[1]
def getPosition(dev, motor_id):
msg = [READ_INFO, motor_id, 0x24, 2]
dev.send(msg)
raw = dev.read(3)
raw_angle = raw[1] * 256 + raw[2]
return raw_angle * 0.29
def setSpeed(dev, motor_id, speed):
#vel = speed * 1.496
msg = [WRITE_INFO, motor_id, 0x20, speed / 256, speed % 256]
dev.send(msg)
raw = dev.read(2)
return raw[1]
|
