Skip to content

Commit 2f09a64

Browse files
ROBOTIS-WillROBOTIS-Will
authored
Merge pull request ROBOTIS-GIT#479 from ROBOTIS-GIT/ROBOTIS-Patch-David-Python
Robotis patch david python
2 parents fc760f2 + 8d757cd commit 2f09a64

File tree

10 files changed

+606
-315
lines changed

10 files changed

+606
-315
lines changed

‎python/tests/protocol2_0/broadcast_ping.py‎

Lines changed: 25 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
#!/usr/bin/env python
22
# -*- coding: utf-8 -*-
33

4-
################################################################################
4+
#*******************************************************************************
55
# Copyright 2017 ROBOTIS CO., LTD.
66
#
77
# Licensed under the Apache License, Version 2.0 (the "License");
@@ -15,18 +15,21 @@
1515
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
1616
# See the License for the specific language governing permissions and
1717
# limitations under the License.
18-
################################################################################
19-
20-
# Author: Ryu Woon Jung (Leon)
21-
22-
#
23-
# ********* broadcastPing Example *********
24-
#
25-
#
26-
# Available Dynamixel model on this example : All models using Protocol 2.0
27-
# This example is tested with two Dynamixel PRO 54-200, and an USB2DYNAMIXEL
28-
# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600)
29-
#
18+
#*******************************************************************************
19+
20+
21+
#*******************************************************************************
22+
#*********************** Broadcast Ping Example ***********************
23+
# Required Environment to run this example :
24+
# - Protocol 2.0 supported DYNAMIXEL(X, P, PRO/PRO(A), MX 2.0 series)
25+
# - DYNAMIXEL Starter Set (U2D2, U2D2 PHB, 12V SMPS)
26+
# How to use the example :
27+
# - Build and Run from proper architecture subdirectory.
28+
# - For ARM based SBCs such as Raspberry Pi, use linux_sbc subdirectory to build and run.
29+
# - https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/
30+
# Author: Ryu Woon Jung (Leon)
31+
# Maintainer : Zerom, Will Son
32+
# *******************************************************************************
3033

3134
import os
3235

@@ -48,13 +51,16 @@ def getch():
4851

4952
from dynamixel_sdk import * # Uses Dynamixel SDK library
5053

51-
# Protocol version
52-
PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel
54+
# DYNAMIXEL Protocol Version (1.0 / 2.0)
55+
# https://emanual.robotis.com/docs/en/dxl/protocol2/
56+
PROTOCOL_VERSION = 2.0
57+
58+
# Define the proper baudrate to search DYNAMIXELs. Note that XL320's baudrate is 1 M bps.
59+
BAUDRATE = 57600
5360

54-
# Default setting
55-
BAUDRATE = 57600 # Dynamixel default baudrate : 57600
56-
DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller
57-
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
61+
# Use the actual port assigned to the U2D2.
62+
# ex) Windows: "COM*", Linux: "/dev/ttyUSB*", Mac: "/dev/tty.usbserial-*"
63+
DEVICENAME = '/dev/ttyUSB0'
5864

5965
# Initialize PortHandler instance
6066
# Set the port path

‎python/tests/protocol2_0/bulk_read_write.py‎

Lines changed: 76 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -17,16 +17,19 @@
1717
# limitations under the License.
1818
################################################################################
1919

20-
# Author: Ryu Woon Jung (Leon)
21-
22-
#
23-
# ********* Bulk Read and Bulk Write Example *********
24-
#
25-
#
26-
# Available Dynamixel model on this example : All models using Protocol 2.0
27-
# This example is tested with two Dynamixel PRO 54-200, and an USB2DYNAMIXEL
28-
# Be sure that Dynamixel PRO properties are already set as %% ID : 1 and 2 / Baudnum : 1 (Baudrate : 57600)
29-
#
20+
#*******************************************************************************
21+
#*********************** Bulk Read and Bulk Write Example ***********************
22+
# Required Environment to run this example :
23+
# - Protocol 2.0 supported DYNAMIXEL(X, P, PRO/PRO(A), MX 2.0 series). Note that the XL320 does not support Bulk Read and Bulk Write.
24+
# - DYNAMIXEL Starter Set (U2D2, U2D2 PHB, 12V SMPS)
25+
# How to use the example :
26+
# - Select the DYNAMIXEL in use at the MY_DXL in the example code.
27+
# - Build and Run from proper architecture subdirectory.
28+
# - For ARM based SBCs such as Raspberry Pi, use linux_sbc subdirectory to build and run.
29+
# - https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/
30+
# Author: Ryu Woon Jung (Leon)
31+
# Maintainer : Zerom, Will Son
32+
# *******************************************************************************
3033

3134
import os
3235

@@ -48,31 +51,64 @@ def getch():
4851

4952
from dynamixel_sdk import * # Uses Dynamixel SDK library
5053

51-
# Control table address
52-
ADDR_PRO_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model
53-
ADDR_PRO_LED_RED = 65
54-
ADDR_PRO_GOAL_POSITION = 116
55-
ADDR_PRO_PRESENT_POSITION = 132
56-
57-
# Data Byte Length
58-
LEN_PRO_LED_RED = 1
59-
LEN_PRO_GOAL_POSITION = 4
60-
LEN_PRO_PRESENT_POSITION = 4
6154

62-
# Protocol version
63-
PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel
55+
#********* DYNAMIXEL Model definition *********
56+
#***** (Use only one definition at a time) *****
57+
MY_DXL = 'X_SERIES' # X330 (5.0 V recommended), X430, X540, 2X430
58+
# MY_DXL = 'MX_SERIES' # MX series with 2.0 firmware update.
59+
# MY_DXL = 'PRO_SERIES' # H54, H42, M54, M42, L54, L42
60+
# MY_DXL = 'PRO_A_SERIES' # PRO series with (A) firmware update.
61+
# MY_DXL = 'P_SERIES' # PH54, PH42, PM54
6462

65-
# Default setting
63+
# Control table address
64+
if MY_DXL == 'X_SERIES' or MY_DXL == 'MX_SERIES':
65+
ADDR_TORQUE_ENABLE = 64
66+
ADDR_LED_RED = 65
67+
LEN_LED_RED = 1 # Data Byte Length
68+
ADDR_GOAL_POSITION = 116
69+
LEN_GOAL_POSITION = 4 # Data Byte Length
70+
ADDR_PRESENT_POSITION = 132
71+
LEN_PRESENT_POSITION = 4 # Data Byte Length
72+
DXL_MINIMUM_POSITION_VALUE = 0 # Refer to the Minimum Position Limit of product eManual
73+
DXL_MAXIMUM_POSITION_VALUE = 4095 # Refer to the Maximum Position Limit of product eManual
74+
BAUDRATE = 57600
75+
elif MY_DXL == 'PRO_SERIES':
76+
ADDR_TORQUE_ENABLE = 562 # Control table address is different in DYNAMIXEL model
77+
ADDR_LED_RED = 563 # R.G.B Address: 563 (red), 564 (green), 565 (blue)
78+
LEN_LED_RED = 1 # Data Byte Length
79+
ADDR_GOAL_POSITION = 596
80+
LEN_GOAL_POSITION = 4
81+
ADDR_PRESENT_POSITION = 611
82+
LEN_PRESENT_POSITION = 4
83+
DXL_MINIMUM_POSITION_VALUE = -150000 # Refer to the Minimum Position Limit of product eManual
84+
DXL_MAXIMUM_POSITION_VALUE = 150000 # Refer to the Maximum Position Limit of product eManual
85+
BAUDRATE = 57600
86+
elif MY_DXL == 'P_SERIES' or MY_DXL == 'PRO_A_SERIES':
87+
ADDR_TORQUE_ENABLE = 512 # Control table address is different in DYNAMIXEL model
88+
ADDR_LED_RED = 513 # R.G.B Address: 513 (red), 544 (green), 515 (blue)
89+
LEN_LED_RED = 1 # Data Byte Length
90+
ADDR_GOAL_POSITION = 564
91+
LEN_GOAL_POSITION = 4 # Data Byte Length
92+
ADDR_PRESENT_POSITION = 580
93+
LEN_PRESENT_POSITION = 4 # Data Byte Length
94+
DXL_MINIMUM_POSITION_VALUE = -150000 # Refer to the Minimum Position Limit of product eManual
95+
DXL_MAXIMUM_POSITION_VALUE = 150000 # Refer to the Maximum Position Limit of product eManual
96+
BAUDRATE = 57600
97+
98+
# DYNAMIXEL Protocol Version (1.0 / 2.0)
99+
# https://emanual.robotis.com/docs/en/dxl/protocol2/
100+
PROTOCOL_VERSION = 2.0
101+
102+
# Make sure that each DYNAMIXEL ID should have unique ID.
66103
DXL1_ID = 1 # Dynamixel#1 ID : 1
67104
DXL2_ID = 2 # Dynamixel#1 ID : 2
68-
BAUDRATE = 57600 # Dynamixel default baudrate : 57600
69-
DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller
70-
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
105+
106+
# Use the actual port assigned to the U2D2.
107+
# ex) Windows: "COM*", Linux: "/dev/ttyUSB*", Mac: "/dev/tty.usbserial-*"
108+
DEVICENAME = '/dev/ttyUSB0'
71109

72110
TORQUE_ENABLE = 1 # Value for enabling the torque
73111
TORQUE_DISABLE = 0 # Value for disabling the torque
74-
DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value
75-
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.)
76112
DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold
77113

78114
index = 0
@@ -116,7 +152,7 @@ def getch():
116152

117153

118154
# Enable Dynamixel#1 Torque
119-
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
155+
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
120156
if dxl_comm_result != COMM_SUCCESS:
121157
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
122158
elif dxl_error != 0:
@@ -125,7 +161,7 @@ def getch():
125161
print("Dynamixel#%d has been successfully connected" % DXL1_ID)
126162

127163
# Enable Dynamixel#2 Torque
128-
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
164+
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
129165
if dxl_comm_result != COMM_SUCCESS:
130166
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
131167
elif dxl_error != 0:
@@ -134,13 +170,13 @@ def getch():
134170
print("Dynamixel#%d has been successfully connected" % DXL2_ID)
135171

136172
# Add parameter storage for Dynamixel#1 present position
137-
dxl_addparam_result = groupBulkRead.addParam(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)
173+
dxl_addparam_result = groupBulkRead.addParam(DXL1_ID, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION)
138174
if dxl_addparam_result != True:
139175
print("[ID:%03d] groupBulkRead addparam failed" % DXL1_ID)
140176
quit()
141177

142178
# Add parameter storage for Dynamixel#2 LED value
143-
dxl_addparam_result = groupBulkRead.addParam(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)
179+
dxl_addparam_result = groupBulkRead.addParam(DXL2_ID, ADDR_LED_RED, LEN_LED_RED)
144180
if dxl_addparam_result != True:
145181
print("[ID:%03d] groupBulkRead addparam failed" % DXL2_ID)
146182
quit()
@@ -154,13 +190,13 @@ def getch():
154190
param_goal_position = [DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]))]
155191

156192
# Add Dynamixel#1 goal position value to the Bulkwrite parameter storage
157-
dxl_addparam_result = groupBulkWrite.addParam(DXL1_ID, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION, param_goal_position)
193+
dxl_addparam_result = groupBulkWrite.addParam(DXL1_ID, ADDR_GOAL_POSITION, LEN_GOAL_POSITION, param_goal_position)
158194
if dxl_addparam_result != True:
159195
print("[ID:%03d] groupBulkWrite addparam failed" % DXL1_ID)
160196
quit()
161197

162198
# Add Dynamixel#2 LED value to the Bulkwrite parameter storage
163-
dxl_addparam_result = groupBulkWrite.addParam(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED, [dxl_led_value[index]])
199+
dxl_addparam_result = groupBulkWrite.addParam(DXL2_ID, ADDR_LED_RED, LEN_LED_RED, [dxl_led_value[index]])
164200
if dxl_addparam_result != True:
165201
print("[ID:%03d] groupBulkWrite addparam failed" % DXL2_ID)
166202
quit()
@@ -180,22 +216,22 @@ def getch():
180216
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
181217

182218
# Check if groupbulkread data of Dynamixel#1 is available
183-
dxl_getdata_result = groupBulkRead.isAvailable(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)
219+
dxl_getdata_result = groupBulkRead.isAvailable(DXL1_ID, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION)
184220
if dxl_getdata_result != True:
185221
print("[ID:%03d] groupBulkRead getdata failed" % DXL1_ID)
186222
quit()
187223

188224
# Check if groupbulkread data of Dynamixel#2 is available
189-
dxl_getdata_result = groupBulkRead.isAvailable(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)
225+
dxl_getdata_result = groupBulkRead.isAvailable(DXL2_ID, ADDR_LED_RED, LEN_LED_RED)
190226
if dxl_getdata_result != True:
191227
print("[ID:%03d] groupBulkRead getdata failed" % DXL2_ID)
192228
quit()
193229

194230
# Get present position value
195-
dxl1_present_position = groupBulkRead.getData(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)
231+
dxl1_present_position = groupBulkRead.getData(DXL1_ID, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION)
196232

197233
# Get LED value
198-
dxl2_led_value_read = groupBulkRead.getData(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)
234+
dxl2_led_value_read = groupBulkRead.getData(DXL2_ID, ADDR_LED_RED, LEN_LED_RED)
199235

200236
print("[ID:%03d] Present Position : %d \t [ID:%03d] LED Value: %d" % (DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_led_value_read))
201237

@@ -212,14 +248,14 @@ def getch():
212248
groupBulkRead.clearParam()
213249

214250
# Disable Dynamixel#1 Torque
215-
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
251+
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
216252
if dxl_comm_result != COMM_SUCCESS:
217253
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
218254
elif dxl_error != 0:
219255
print("%s" % packetHandler.getRxPacketError(dxl_error))
220256

221257
# Disable Dynamixel#2 Torque
222-
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
258+
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
223259
if dxl_comm_result != COMM_SUCCESS:
224260
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
225261
elif dxl_error != 0:

0 commit comments

Comments
 (0)