n_Bulk_Read_Write.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 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 /* Authors: Taehun Lim (Darby) */
18 
19 #include <DynamixelWorkbench.h>
20 
21 void swap(int32_t *array);
22 
23 int main(int argc, char *argv[])
24 {
25  const char* port_name = "/dev/ttyUSB0";
26  int baud_rate = 57600;
27 
28  uint16_t model_number = 0;
29  uint8_t dxl_id[2] = {0, 0};
30 
31  if (argc < 5)
32  {
33  printf("Please set '-port_name', '-baud_rate', '-dynamixel_id_1', '-dynamixel_id_2' arguments for connected Dynamixels\n");
34  return 0;
35  }
36  else
37  {
38  port_name = argv[1];
39  baud_rate = atoi(argv[2]);
40  dxl_id[0] = atoi(argv[3]);
41  dxl_id[1] = atoi(argv[4]);
42  }
43 
45 
46  const char *log;
47  bool result = false;
48 
49  result = dxl_wb.init(port_name, baud_rate, &log);
50  if (result == false)
51  {
52  printf("%s\n", log);
53  printf("Failed to init\n");
54 
55  return 0;
56  }
57  else
58  printf("Succeed to init(%d)\n", baud_rate);
59 
60  for (int cnt = 0; cnt < 2; cnt++)
61  {
62  result = dxl_wb.ping(dxl_id[cnt], &model_number, &log);
63  if (result == false)
64  {
65  printf("%s\n", log);
66  printf("Failed to ping\n");
67  }
68  else
69  {
70  printf("Succeeded to ping\n");
71  printf("id : %d, model_number : %d\n", dxl_id[cnt], model_number);
72  }
73 
74  result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log);
75  if (result == false)
76  {
77  printf("%s\n", log);
78  printf("Failed to change joint mode\n");
79  }
80  else
81  {
82  printf("Succeed to change joint mode\n");
83  }
84  }
85 
86  result = dxl_wb.initBulkWrite(&log);
87  if (result == false)
88  {
89  printf("%s\n", log);
90  }
91  else
92  {
93  printf("%s\n", log);
94  }
95 
96  result = dxl_wb.initBulkRead(&log);
97  if (result == false)
98  {
99  printf("%s\n", log);
100  }
101  else
102  {
103  printf("%s\n", log);
104  }
105 
106  result = dxl_wb.addBulkReadParam(dxl_id[0], "Present_Position", &log);
107  if (result == false)
108  {
109  printf("%s\n", log);
110  printf("Failed to add bulk read position param\n");
111  }
112  else
113  {
114  printf("%s\n", log);
115  }
116 
117  result = dxl_wb.addBulkReadParam(dxl_id[1], "LED", &log);
118  if (result == false)
119  {
120  printf("%s\n", log);
121  printf("Failed to add bulk read led param\n");
122  }
123  else
124  {
125  printf("%s\n", log);
126  }
127 
128  int32_t goal_position[2] = {0, 1023};
129  int32_t led[2] = {0, 1};
130 
131  int32_t get_data[2] = {0, 0};
132 
133  const uint8_t handler_index = 0;
134 
135  while(1)
136  {
137  result = dxl_wb.addBulkWriteParam(dxl_id[0], "Goal_Position", goal_position[0], &log);
138  if (result == false)
139  {
140  printf("%s\n", log);
141  printf("Failed to add bulk write position param\n");
142  }
143  else
144  {
145  printf("%s\n", log);
146  }
147 
148  result = dxl_wb.addBulkWriteParam(dxl_id[1], "LED", led[0], &log);
149  if (result == false)
150  {
151  printf("%s\n", log);
152  printf("Failed to add bulk write led param\n");
153  }
154  else
155  {
156  printf("%s\n", log);
157  }
158 
159  result = dxl_wb.bulkWrite(&log);
160  if (result == false)
161  {
162  printf("%s\n", log);
163  printf("Failed to bulk write\n");
164  }
165 
166  do
167  {
168  result = dxl_wb.bulkRead(&log);
169  if (result == false)
170  {
171  printf("%s\n", log);
172  printf("Failed to bulk read\n");
173  }
174 
175  result = dxl_wb.getBulkReadData(&get_data[0], &log);
176  if (result == false)
177  {
178  printf("%s\n", log);
179  }
180  else
181  {
182  printf("[ID %d]\tGoal Position : %d\tPresent Position : %d, [ID %d]\tLED : %d\n"
183  ,dxl_id[0], goal_position[0], get_data[0], dxl_id[1], get_data[1]);
184  }
185 
186  }while(abs(goal_position[0] - get_data[0]) > 15);
187 
188  swap(goal_position);
189  swap(led);
190  }
191 
192  return 0;
193 }
194 
195 void swap(int32_t *array)
196 {
197  int32_t tmp = array[0];
198  array[0] = array[1];
199  array[1] = tmp;
200 }
bool addBulkWriteParam(uint8_t id, uint16_t address, uint16_t length, int32_t data, const char **log=NULL)
bool addBulkReadParam(uint8_t id, uint16_t address, uint16_t length, const char **log=NULL)
void swap(int32_t *array)
bool getBulkReadData(int32_t *data, const char **log=NULL)
bool init(const char *device_name="/dev/ttyUSB0", uint32_t baud_rate=57600, const char **log=NULL)
DynamixelWorkbench dxl_wb
Definition: p_Monitor.cpp:40
bool ping(uint8_t id, uint16_t *get_model_number, const char **log=NULL)
bool initBulkWrite(const char **log=NULL)
bool initBulkRead(const char **log=NULL)
bool bulkWrite(const char **log=NULL)
bool bulkRead(const char **log=NULL)
int main(int argc, char *argv[])
bool jointMode(uint8_t id, int32_t velocity=0, int32_t acceleration=0, const char **log=NULL)


dynamixel_workbench_toolbox
Author(s): Darby Lim , Ryan Shim
autogenerated on Mon Sep 28 2020 03:37:05