l_Sync_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.addSyncWriteHandler(dxl_id[0], "Goal_Position", &log);
87  if (result == false)
88  {
89  printf("%s\n", log);
90  printf("Failed to add sync write handler\n");
91  }
92 
93  int32_t goal_position[2] = {0, 1023};
94 
95  const uint8_t handler_index = 0;
96 
97  while(1)
98  {
99  result = dxl_wb.syncWrite(handler_index, &goal_position[0], &log);
100  if (result == false)
101  {
102  printf("%s\n", log);
103  printf("Failed to sync write position\n");
104  }
105 
106  sleep(3);
107 
108  swap(goal_position);
109  }
110 
111  return 0;
112 }
113 
114 void swap(int32_t *array)
115 {
116  int32_t tmp = array[0];
117  array[0] = array[1];
118  array[1] = tmp;
119 }
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 syncWrite(uint8_t index, int32_t *data, const char **log=NULL)
void swap(int32_t *array)
bool addSyncWriteHandler(uint16_t address, uint16_t length, const char **log=NULL)
int main(int argc, char *argv[])
bool ping(uint8_t id, uint16_t *get_model_number, const char **log=NULL)
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