forked from ROBOTIS-GIT/DynamixelSDK
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathSyncWrite.java
More file actions
241 lines (206 loc) · 10 KB
/
SyncWrite.java
File metadata and controls
241 lines (206 loc) · 10 KB
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
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
/*******************************************************************************
* Copyright 2017 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/* Author: Ryu Woon Jung (Leon) */
//
// ********* Sync Write Example *********
//
//
// Available Dynamixel model on this example : All models using Protocol 1.0
// This example is designed for using two Dynamixel MX-28, and an USB2DYNAMIXEL.
// To use another Dynamixel model, such as X series, see their details in E-Manual(emanual.robotis.com) and edit below variables yourself.
// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600)
//
import java.util.Scanner;
public class SyncWrite
{
public static void main(String[] args)
{
// Control table address
short ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model
short ADDR_MX_GOAL_POSITION = 30;
short ADDR_MX_PRESENT_POSITION = 36;
// Data Byte Length
short LEN_MX_GOAL_POSITION = 2;
// Protocol version
int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel
// Default setting
byte DXL1_ID = 1; // Dynamixel ID: 1
byte DXL2_ID = 2; // Dynamixel ID: 2
int BAUDRATE = 57600;
String DEVICENAME = "/dev/ttyUSB0"; // Check which port is being used on your controller
// ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
byte TORQUE_ENABLE = 1; // Value for enabling the torque
byte TORQUE_DISABLE = 0; // Value for disabling the torque
short DXL_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value
short 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.)
int DXL_MOVING_STATUS_THRESHOLD = 10; // Dynamixel moving status threshold
String KEY_FOR_ESCAPE = "e"; // Key for escape
int COMM_SUCCESS = 0; // Communication Success result value
int COMM_TX_FAIL = -1001; // Communication Tx Failed
// Instead of getch
Scanner scanner = new Scanner(System.in);
// Initialize Dynamixel class for java
Dynamixel dynamixel = new Dynamixel();
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = dynamixel.portHandler(DEVICENAME);
// Initialize PacketHandler Structs
dynamixel.packetHandler();
// Initialize Groupsyncwrite instance
int group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION);
int index = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
Boolean dxl_addparam_result = false; // AddParam result
short[] dxl_goal_position = new short[]{DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE}; // Goal position
byte dxl_error = 0; // Dynamixel error
short dxl1_present_position = 0, dxl2_present_position = 0; // Present position
// Open port
if (dynamixel.openPort(port_num))
{
System.out.println("Succeeded to open the port!");
}
else
{
System.out.println("Failed to open the port!");
System.out.println("Press any key to terminate...");
scanner.nextLine();
return;
}
// Set port baudrate
if (dynamixel.setBaudRate(port_num, BAUDRATE))
{
System.out.println("Succeeded to change the baudrate!");
}
else
{
System.out.println("Failed to change the baudrate!");
System.out.println("Press any key to terminate...");
scanner.nextLine();
return;
}
// Enable Dynamixel#1 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
}
else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error));
}
else
{
System.out.printf("Dynamixel#%d has been successfully connected\n", DXL1_ID);
}
// Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
}
else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error));
}
else
{
System.out.printf("Dynamixel#%d has been successfully connected\n", DXL2_ID);
}
while (true)
{
System.out.println("Press enter to continue! (or press e then enter to quit!)");
if(scanner.nextLine().equals(KEY_FOR_ESCAPE))
break;
// Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL1_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION);
if (dxl_addparam_result != true)
{
System.out.printf("[ID: %d] groupSyncWrite addparam failed\n", DXL1_ID);
return;
}
// Add Dynamixel#2 goal position value to the Syncwrite parameter storage
dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION);
if (dxl_addparam_result != true)
{
System.out.printf("[ID: %d] groupSyncWrite addparam failed\n", DXL2_ID);
return;
}
// Syncwrite goal position
dynamixel.groupSyncWriteTxPacket(group_num);
if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
// Clear syncwrite parameter storage
dynamixel.groupSyncWriteClearParam(group_num);
do
{
// Read Dynamixel#1 present position
dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION);
if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
}
else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error));
}
// Read Dynamixel#2 present position
dxl2_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION);
if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
}
else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error));
}
System.out.printf("[ID: %d] GoalPos:%d PresPos:%d [ID: %d] GoalPos:%d PresPos:%d\n", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position);
} while ((Math.abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (Math.abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD));
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel#1 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
}
else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error));
}
// Disable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
}
else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error));
}
// Close port
dynamixel.closePort(port_num);
return;
}
}