Skip to content

Commit be4a67e

Browse files
honghyun79honghyun79
committed
add: fast_sync_read example
1 parent 21b6579 commit be4a67e

File tree

13 files changed

+1074
-0
lines changed

13 files changed

+1074
-0
lines changed

‎c++/example/protocol2.0/fast_sync_read/fast_sync_read.cpp‎

Lines changed: 331 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,331 @@
1+
/*******************************************************************************
2+
* Copyright 2017 ROBOTIS CO., LTD.
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*******************************************************************************/
16+
17+
/* Author: Ryu Woon Jung (Leon), Honghyun Kim */
18+
19+
//
20+
// ********* Sync Read and Sync Write Example *********
21+
//
22+
//
23+
// Available DYNAMIXEL model on this example : All models using Protocol 2.0
24+
// This example is tested with two DYNAMIXEL P Series, and an U2D2
25+
// Be sure that DYNAMIXEL P properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600)
26+
//
27+
28+
#if defined(__linux__) || defined(__APPLE__)
29+
#include <fcntl.h>
30+
#include <termios.h>
31+
#define STDIN_FILENO 0
32+
#elif defined(_WIN32) || defined(_WIN64)
33+
#include <conio.h>
34+
#endif
35+
36+
#include <stdlib.h>
37+
#include <stdio.h>
38+
39+
#include "dynamixel_sdk.h" // Uses DYNAMIXEL SDK library
40+
41+
// Control table address
42+
#define ADDR_PRO_TORQUE_ENABLE 512 // Control table address is different in DYNAMIXEL model
43+
#define ADDR_PRO_GOAL_POSITION 564
44+
#define ADDR_PRO_PRESENT_POSITION 580
45+
46+
// Data Byte Length
47+
#define LEN_PRO_GOAL_POSITION 4
48+
#define LEN_PRO_PRESENT_POSITION 4
49+
50+
// Protocol version
51+
#define PROTOCOL_VERSION 2.0 // See which protocol version is used in the DYNAMIXEL
52+
53+
// Default setting
54+
#define DXL1_ID 1 // DYNAMIXEL#1 ID: 1
55+
#define DXL2_ID 2 // DYNAMIXEL#2 ID: 2
56+
#define BAUDRATE 57600
57+
#define DEVICENAME "COM3" // Check which port is being used on your controller
58+
// ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
59+
60+
#define TORQUE_ENABLE 1 // Value for enabling the torque
61+
#define TORQUE_DISABLE 0 // Value for disabling the torque
62+
#define DXL_MINIMUM_POSITION_VALUE -150000 // DYNAMIXEL will rotate between this value
63+
#define DXL_MAXIMUM_POSITION_VALUE 150000 // 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.)
64+
#define DXL_MOVING_STATUS_THRESHOLD 20 // DYNAMIXEL moving status threshold
65+
66+
#define ESC_ASCII_VALUE 0x1b
67+
68+
int getch()
69+
{
70+
#if defined(__linux__) || defined(__APPLE__)
71+
struct termios oldt, newt;
72+
int ch;
73+
tcgetattr(STDIN_FILENO, &oldt);
74+
newt = oldt;
75+
newt.c_lflag &= ~(ICANON | ECHO);
76+
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
77+
ch = getchar();
78+
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
79+
return ch;
80+
#elif defined(_WIN32) || defined(_WIN64)
81+
return _getch();
82+
#endif
83+
}
84+
85+
int kbhit(void)
86+
{
87+
#if defined(__linux__) || defined(__APPLE__)
88+
struct termios oldt, newt;
89+
int ch;
90+
int oldf;
91+
92+
tcgetattr(STDIN_FILENO, &oldt);
93+
newt = oldt;
94+
newt.c_lflag &= ~(ICANON | ECHO);
95+
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
96+
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
97+
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
98+
99+
ch = getchar();
100+
101+
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
102+
fcntl(STDIN_FILENO, F_SETFL, oldf);
103+
104+
if (ch != EOF)
105+
{
106+
ungetc(ch, stdin);
107+
return 1;
108+
}
109+
110+
return 0;
111+
#elif defined(_WIN32) || defined(_WIN64)
112+
return _kbhit();
113+
#endif
114+
}
115+
116+
int main()
117+
{
118+
// Initialize PortHandler instance
119+
// Set the port path
120+
// Get methods and members of PortHandlerLinux or PortHandlerWindows
121+
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
122+
123+
// Initialize PacketHandler instance
124+
// Set the protocol version
125+
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
126+
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
127+
128+
// Initialize GroupSyncWrite instance
129+
dynamixel::GroupSyncWrite groupSyncWrite(portHandler, packetHandler, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION);
130+
131+
// Initialize GroupFastSyncRead instance for Present Position
132+
dynamixel::GroupFastSyncRead groupFastSyncRead(portHandler, packetHandler, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
133+
134+
int index = 0;
135+
int dxl_comm_result = COMM_TX_FAIL; // Communication result
136+
bool dxl_addparam_result = false; // addParam result
137+
bool dxl_getdata_result = false; // GetParam result
138+
int dxl_goal_position[2] = {DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE}; // Goal position
139+
140+
uint8_t dxl_error = 0; // DYNAMIXEL error
141+
uint8_t param_goal_position[4];
142+
int32_t dxl1_present_position = 0, dxl2_present_position = 0; // Present position
143+
144+
// Open port
145+
if (portHandler->openPort())
146+
{
147+
printf("Succeeded to open the port!\n");
148+
}
149+
else
150+
{
151+
printf("Failed to open the port!\n");
152+
printf("Press any key to terminate...\n");
153+
getch();
154+
return 0;
155+
}
156+
157+
// Set port baudrate
158+
if (portHandler->setBaudRate(BAUDRATE))
159+
{
160+
printf("Succeeded to change the baudrate!\n");
161+
}
162+
else
163+
{
164+
printf("Failed to change the baudrate!\n");
165+
printf("Press any key to terminate...\n");
166+
getch();
167+
return 0;
168+
}
169+
170+
// Enable DYNAMIXEL#1 Torque
171+
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
172+
if (dxl_comm_result != COMM_SUCCESS)
173+
{
174+
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
175+
}
176+
else if (dxl_error != 0)
177+
{
178+
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
179+
}
180+
else
181+
{
182+
printf("DYNAMIXEL#%d has been successfully connected \n", DXL1_ID);
183+
}
184+
185+
// Enable DYNAMIXEL#2 Torque
186+
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
187+
if (dxl_comm_result != COMM_SUCCESS)
188+
{
189+
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
190+
}
191+
else if (dxl_error != 0)
192+
{
193+
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
194+
}
195+
else
196+
{
197+
printf("DYNAMIXEL#%d has been successfully connected \n", DXL2_ID);
198+
}
199+
200+
// Add parameter storage for DYNAMIXEL#1 present position value
201+
dxl_addparam_result = groupFastSyncRead.addParam(DXL1_ID);
202+
if (dxl_addparam_result != true)
203+
{
204+
fprintf(stderr, "[ID:%03d] groupSyncRead addparam failed", DXL1_ID);
205+
return 0;
206+
}
207+
208+
// Add parameter storage for Dynamixel#2 present position value
209+
dxl_addparam_result = groupFastSyncRead.addParam(DXL2_ID);
210+
if (dxl_addparam_result != true)
211+
{
212+
fprintf(stderr, "[ID:%03d] groupSyncRead addparam failed", DXL2_ID);
213+
return 0;
214+
}
215+
216+
while(1)
217+
{
218+
printf("Press any key to continue! (or press ESC to quit!)\n");
219+
if (getch() == ESC_ASCII_VALUE)
220+
break;
221+
222+
// Allocate goal position value into byte array
223+
param_goal_position[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index]));
224+
param_goal_position[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index]));
225+
param_goal_position[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index]));
226+
param_goal_position[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]));
227+
228+
// Add DYNAMIXEL#1 goal position value to the Syncwrite storage
229+
dxl_addparam_result = groupSyncWrite.addParam(DXL1_ID, param_goal_position);
230+
if (dxl_addparam_result != true)
231+
{
232+
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed", DXL1_ID);
233+
return 0;
234+
}
235+
236+
// Add DYNAMIXEL#2 goal position value to the Syncwrite parameter storage
237+
dxl_addparam_result = groupSyncWrite.addParam(DXL2_ID, param_goal_position);
238+
if (dxl_addparam_result != true)
239+
{
240+
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed", DXL2_ID);
241+
return 0;
242+
}
243+
244+
// Syncwrite goal position
245+
dxl_comm_result = groupSyncWrite.txPacket();
246+
if (dxl_comm_result != COMM_SUCCESS) printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
247+
248+
// Clear syncwrite parameter storage
249+
groupSyncWrite.clearParam();
250+
251+
do
252+
{
253+
// Syncread present position
254+
dxl_comm_result = groupFastSyncRead.txRxPacket();
255+
if (dxl_comm_result != COMM_SUCCESS)
256+
{
257+
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
258+
}
259+
else if (groupFastSyncRead.getError(DXL1_ID, &dxl_error))
260+
{
261+
printf("[ID:%03d] %s\n", DXL1_ID, packetHandler->getRxPacketError(dxl_error));
262+
}
263+
else if (groupFastSyncRead.getError(DXL2_ID, &dxl_error))
264+
{
265+
printf("[ID:%03d] %s\n", DXL2_ID, packetHandler->getRxPacketError(dxl_error));
266+
}
267+
268+
// Check if groupFastSyncRead data of DYNAMIXEL#1 is available
269+
dxl_getdata_result = groupFastSyncRead.isAvailable(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
270+
if (dxl_getdata_result != true)
271+
{
272+
fprintf(stderr, "[ID:%03d] groupFastSyncRead getdata failed", DXL1_ID);
273+
return 0;
274+
}
275+
276+
// Check if groupFastSyncRead data of DYNAMIXEL#2 is available
277+
dxl_getdata_result = groupFastSyncRead.isAvailable(DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
278+
if (dxl_getdata_result != true)
279+
{
280+
fprintf(stderr, "[ID:%03d] groupFastSyncRead getdata failed", DXL2_ID);
281+
return 0;
282+
}
283+
284+
// Get DYNAMIXEL#1 present position value
285+
dxl1_present_position = groupFastSyncRead.getData(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
286+
287+
// Get DYNAMIXEL#2 present position value
288+
dxl2_present_position = groupFastSyncRead.getData(DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
289+
290+
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position);
291+
292+
}while((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD));
293+
294+
// Change goal position
295+
if (index == 0)
296+
{
297+
index = 1;
298+
}
299+
else
300+
{
301+
index = 0;
302+
}
303+
}
304+
305+
// Disable DYNAMIXEL#1 Torque
306+
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
307+
if (dxl_comm_result != COMM_SUCCESS)
308+
{
309+
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
310+
}
311+
else if (dxl_error != 0)
312+
{
313+
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
314+
}
315+
316+
// Disable DYNAMIXEL#2 Torque
317+
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
318+
if (dxl_comm_result != COMM_SUCCESS)
319+
{
320+
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
321+
}
322+
else if (dxl_error != 0)
323+
{
324+
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
325+
}
326+
327+
// Close port
328+
portHandler->closePort();
329+
330+
return 0;
331+
}

0 commit comments

Comments
 (0)