-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdynamixel.py
142 lines (126 loc) · 7.05 KB
/
dynamixel.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
#!/usr/bin/env python
import os
import glob
import ctypes
import dynamixel_functions as dynamixel
# Dynamixel moving status threshold
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
def get_available_ports():
return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*") + glob.glob("/dev/ttyCOM*")
class Dxl:
def __init__(self, DeviceName):
self.portHandler = dynamixel.portHandler(DeviceName)
self.packetHandler = dynamixel.packetHandler()
self.dxl_comm_result = COMM_TX_FAIL
self.dxl_error = 0
self.dxl_addparam_result = 0
self.LEN_MX_GOAL_POSITION = 2
self.LEN_MX_PRESENT_POSITION = 2
self.ADDR_MX_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model
self.ADDR_MX_GOAL_POSITION = 30
self.ADDR_MX_PRESENT_POSITION = 36
self.ADDR_MX_SPEED = 32
self.PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel
self.groupSync = dynamixel.groupSyncWrite(self.portHandler, self.PROTOCOL_VERSION, self.ADDR_MX_GOAL_POSITION, self.LEN_MX_GOAL_POSITION)
# Protocol version
# Default setting
self.BAUDRATE = 1000000
self.TORQUE_ENABLE = 1 # Value for enabling the torque
self.TORQUE_DISABLE = 0 # Value for disabling the torque
self.DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value
self.DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
self.DXL_MOVING_STATUS_THRESHOLD = 10
if(dynamixel.openPort(self.portHandler)):
print "Port open done"
else:
print("Failed to open port")
quit()
if(dynamixel.setBaudRate(self.portHandler, self.BAUDRATE)):
print "Change baudrate succeeded"
else:
print "Cannot add baudrate"
quit()
def scan(self, ran = 254):
_ids = []
for i in range(ran):
dynamixel.ping(self.portHandler, self.PROTOCOL_VERSION, i)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
pass
elif(dxl_error != 0):
pass
else:
_ids.append(i)
return _ids
def _write(self, DXL_ID, angle):
dynamixel.write2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, self.ADDR_MX_GOAL_POSITION, 2048 + int(angle / 0.088))
dxl_comm_result= dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
elif(dxl_error != 0):
print(dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_error))
def _read(self, DXL_ID):
dxl_present_position = dynamixel.read2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, self.ADDR_MX_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
elif(dxl_error != 0):
print(dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_error))
return (dxl_present_position - 2048) * 0.088
def set_goal_position(self, ids):
# for i, angle in ids.iteritems():
# self._write(i,angle)
for i, angle in ids.iteritems():
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(self.groupSync, i, 2048 + int(angle / 0.088), self.LEN_MX_GOAL_POSITION)).value
dynamixel.groupSyncWriteTxPacket(self.groupSync)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
dynamixel.groupSyncWriteClearParam(self.groupSync)
def get_present_position(self, ids):
present_pos = {}
for i in ids:
present_pos[i] = self._read(i)
return present_pos
def _set_moving_speed(self, DXL_ID, speed):
dynamixel.write2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, self.ADDR_MX_SPEED, int(speed * 1024 / 1000))
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
elif(dxl_error != 0):
print(dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_error))
def set_moving_speed(self, ids):
for i,val in ids.iteritems():
self._set_moving_speed(i,val)
def _enable_torque(self, DXL_ID):
dynamixel.write2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, self.ADDR_MX_TORQUE_ENABLE, 1)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
elif(dxl_error != 0):
print(dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_error))
def enable_torque(self, ids):
for i in ids:
self._enable_torque(i)
def _disable_torque(self, DXL_ID):
dynamixel.write2ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, self.ADDR_MX_TORQUE_ENABLE, 0)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
elif(dxl_error != 0):
print(dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_error))
def disable_torque(self, ids):
for i in ids:
self._enable_torque(i)
# ports = get_available_ports()
# d = Dxl(ports[0])
# ids = d.scan(30)
# for i in ids:
# d._write(i, 0)