├── dynamixel_workbench_toolbox ├── src │ ├── DynamixelWorkbench.h │ └── dynamixel_workbench_toolbox │ │ ├── dynamixel_tool.cpp │ │ └── dynamixel_workbench.cpp ├── library.properties ├── README.md ├── package.xml ├── examples │ ├── src │ │ ├── b_Ping.cpp │ │ ├── a_Model_Scan.cpp │ │ ├── g_Reset.cpp │ │ ├── f_Reboot.cpp │ │ ├── d_BPS_Change.cpp │ │ ├── o_Find_Dynamixel.cpp │ │ ├── h_Position.cpp │ │ ├── i_Velocity.cpp │ │ ├── k_Read_Write.cpp │ │ ├── j_Current_Based_Position.cpp │ │ ├── c_ID_Change.cpp │ │ ├── l_Sync_Write.cpp │ │ ├── m_Sync_Read_Write.cpp │ │ ├── e_Mode_Change.cpp │ │ ├── n_Bulk_Read_Write.cpp │ │ └── p_Monitor.cpp │ └── CMakeLists.txt ├── include │ └── dynamixel_workbench_toolbox │ │ ├── dynamixel_tool.h │ │ ├── dynamixel_item.h │ │ ├── dynamixel_workbench.h │ │ └── dynamixel_driver.h ├── CMakeLists.txt └── CHANGELOG.rst ├── dynamixel_workbench ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── .github └── workflows │ ├── dynamixel_workbench.repos │ └── ros-ci.yml ├── 99-dynamixel-workbench-cdc.rules ├── .gitignore ├── ISSUE_TEMPLATE.md ├── README.md └── LICENSE /dynamixel_workbench_toolbox/src/DynamixelWorkbench.h: -------------------------------------------------------------------------------- 1 | #include "../include/dynamixel_workbench_toolbox/dynamixel_workbench.h" 2 | -------------------------------------------------------------------------------- /dynamixel_workbench/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(dynamixel_workbench) 3 | find_package(ament_cmake REQUIRED) 4 | ament_package() 5 | -------------------------------------------------------------------------------- /.github/workflows/dynamixel_workbench.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | dynamixel_workbench/dynamixel_workbench_msgs: 3 | type: git 4 | url: https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git 5 | version: main 6 | -------------------------------------------------------------------------------- /99-dynamixel-workbench-cdc.rules: -------------------------------------------------------------------------------- 1 | #http://linux-tips.org/t/prevent-modem-manager-to-capture-usb-serial-devices/284/2. 2 | 3 | #cp rules /etc/udev/rules.d/ 4 | #sudo udevadm control --reload-rules 5 | #sudo udevadm trigger 6 | 7 | KERNEL=="ttyUSB*", DRIVERS=="ftdi_sio", MODE="0666", ATTR{device/latency_timer}="1" 8 | 9 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/library.properties: -------------------------------------------------------------------------------- 1 | name=DynamixelWorkbench 2 | version=2.2.5 3 | author=Darby Lim (thlim@robotis.com) 4 | maintainer=Pyo (pyo@robotis.com) 5 | sentence=DynamixelWorkbench using DynamixelSDK 6 | paragraph=Tools for Dynamixel 7 | category=Other 8 | url=https://github.com/ROBOTIS-GIT/dynamixel-workbench 9 | architectures=OpenCM904 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Users 32 | *.vscode -------------------------------------------------------------------------------- /ISSUE_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | ISSUE TEMPLATE ver. 1.1.0 2 | 3 | Before submit an issue ticket, please refer to [ROBOTIS e-Manual](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/) for more information. 4 | 5 | 1. Which serial interface do you use? (ex, U2D2, USB2DYNAMIXEL, OpenCR, etc...) 6 | 7 | 2. Which DYNAMIXEL and how many are they in use? 8 | - Model Name(s) : 9 | - ID(s) : 10 | - Baud Rate of DYNAMIXEL : 11 | - Protocol Version : 12 | 13 | 3. Please describe the issue. 14 | 15 | 4. Please describe the Steps to reproduce the issue. 16 | 17 | 5. Please Paste any error messages / screenshots 18 | 19 | 6. Please describe how to resolve the issue (if resolved) 20 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/README.md: -------------------------------------------------------------------------------- 1 | How to run dynamixel_workbench in linux without ROS 2 | 3 | 1. Downloads DynamixelSDK 4 | 5 | ``` 6 | $ git clone https://github.com/ROBOTIS-GIT/DynamixelSDK 7 | ``` 8 | 9 | 2. Build DynamixelSDK 10 | 11 | http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/library_setup/cpp_linux/#build-the-library 12 | 13 | 14 | 3. Downloads Dynamixel-Workbench 15 | 16 | ``` 17 | $ git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench 18 | ``` 19 | 20 | 4. Build Dynamixel-Workbench and Run examples 21 | 22 | ``` 23 | $ cd ${YOUR_DOWNLOAD_PATH}/dynamixel_workbench/dynamixel_workbench_toolbox/examples 24 | $ mkdir build && cd build 25 | $ cmake .. 26 | $ make 27 | $ sudo chmod a+rw ${USB_PORT} 28 | $ ./monitor 29 | ``` -------------------------------------------------------------------------------- /dynamixel_workbench/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dynamixel_workbench 5 | 2.2.5 6 | 7 | Dynamixel-Workbench is dynamixel solution for ROS. 8 | This metapackage allows you to easily change the ID, baudrate and operating mode of the Dynamixel. 9 | Furthermore, it supports various controllers based on operating mode and Dynamixel SDK. 10 | These controllers are commanded by operators. 11 | 12 | Pyo 13 | Apache 2.0 14 | http://wiki.ros.org/dynamixel_workbench 15 | https://github.com/ROBOTIS-GIT/dynamixel-workbench 16 | https://github.com/ROBOTIS-GIT/dynamixel-workbench/issues 17 | Darby Lim 18 | Will Son 19 | Ryan Shim 20 | Wonho Yun 21 | ament_cmake 22 | dynamixel_workbench_toolbox 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dynamixel_workbench_toolbox 5 | 2.2.5 6 | 7 | This package is composed of 'dynamixel_item', 'dynamixel_tool', 'dynamixel_driver' and 'dynamixel_workbench' class. 8 | The 'dynamixel_item' is saved as control table item and information of DYNAMIXEL. 9 | The 'dynamixel_tool' class loads its by model number of DYNAMIXEL. 10 | The 'dynamixel_driver' class includes wraped function used in DYNAMIXEL SDK. 11 | The 'dynamixel_workbench' class make simple to use DYNAMIXEL. 12 | 13 | Pyo 14 | Apache 2.0 15 | http://wiki.ros.org/dynamixel_workbench_toolbox 16 | http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/ 17 | https://github.com/ROBOTIS-GIT/dynamixel-workbench 18 | https://github.com/ROBOTIS-GIT/dynamixel-workbench/issues 19 | Darby Lim 20 | Ryan Shim 21 | Will Son 22 | Wonho Yun 23 | ament_cmake 24 | dynamixel_sdk 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DYNAMIXEL Workbench 2 | - Active Branches: noetic, humble, jazzy, main 3 | - Legacy Branches: *-devel 4 | 5 | ## Open Source Projects Related to Dynamixel Workbench 6 | - [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) 7 | - [dynamixel_workbench](https://github.com/ROBOTIS-GIT/dynamixel-workbench) 8 | - [dynamixel_workbench_msgs](https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs) 9 | - [dynamixel_hardware_interface](https://github.com/ROBOTIS-GIT/dynamixel_hardware_interface) 10 | - [dynamixel_interfaces](https://github.com/ROBOTIS-GIT/dynamixel_interfaces) 11 | - [turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3) 12 | - [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) 13 | - [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) 14 | - [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) 15 | 16 | ## Documentation, Videos, and Community 17 | 18 | ### Official Documentation 19 | - ⚙️ **[ROBOTIS DYNAMIXEL](https://dynamixel.com/)** 20 | - 📚 **[ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/)** 21 | - 📚 **[ROBOTIS e-Manual for Dynamixel Workbench](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/)** 22 | - 📚 **[ROBOTIS e-Manual for TurtleBot3](http://turtlebot3.robotis.com/)** 23 | - 📚 **[ROBOTIS e-Manual for OpenMANIPULATOR-X](https://emanual.robotis.com/docs/en/platform/openmanipulator_x/overview/)** 24 | 25 | ### Learning Resources 26 | - 🎥 **[ROBOTIS YouTube Channel](https://www.youtube.com/@ROBOTISCHANNEL)** 27 | - 🎥 **[ROBOTIS Open Source YouTube Channel](https://www.youtube.com/@ROBOTISOpenSourceTeam)** 28 | - 🎥 **[ROBOTIS TurtleBot3 YouTube Playlist](https://www.youtube.com/playlist?list=PLRG6WP3c31_XI3wlvHlx2Mp8BYqgqDURU)** 29 | - 🎥 **[ROBOTIS OpenMANIPULATOR YouTube Playlist](https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb)** 30 | 31 | ### Community & Support 32 | - 💬 **[ROBOTIS Community Forum](https://forum.robotis.com/)** 33 | - 💬 **[TurtleBot category from ROS Community](https://discourse.ros.org/c/turtlebot/)** 34 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/b_Ping.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeeded to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeeded to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | return 0; 70 | } 71 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/a_Model_Scan.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | 26 | if (argc < 3) 27 | { 28 | printf("Please set '-port_name' and '-baud_rate' arguments for connected Dynamixels\n"); 29 | return 0; 30 | } 31 | else 32 | { 33 | port_name = argv[1]; 34 | baud_rate = atoi(argv[2]); 35 | } 36 | 37 | DynamixelWorkbench dxl_wb; 38 | 39 | const char *log; 40 | bool result = false; 41 | 42 | uint8_t scanned_id[16]; 43 | uint8_t dxl_cnt = 0; 44 | uint8_t range = 100; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeeded to init(%d)\n", baud_rate); 56 | 57 | printf("Wait for scan...\n"); 58 | result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log); 59 | if (result == false) 60 | { 61 | printf("%s\n", log); 62 | printf("Failed to scan\n"); 63 | } 64 | else 65 | { 66 | printf("Find %d Dynamixels\n", dxl_cnt); 67 | 68 | for (int cnt = 0; cnt < dxl_cnt; cnt++) 69 | printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); 70 | } 71 | 72 | return 0; 73 | } 74 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/g_Reset.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.reset(dxl_id, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to reset\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to reset\n"); 78 | } 79 | 80 | return 0; 81 | } 82 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/f_Reboot.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.reboot(dxl_id, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to reboot\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to reboot\n"); 78 | } 79 | 80 | return 0; 81 | } 82 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/d_BPS_Change.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int dxl_id = 1; 25 | int baud_rate = 57600; 26 | int new_baud_rate = 1000000; 27 | 28 | if (argc < 5) 29 | { 30 | printf("Please set '-port_name', '-baud_rate', '-dynamixel_id', '-new_baud_rate' arguments for connected Dynamixels\n"); 31 | return 0; 32 | } 33 | else 34 | { 35 | port_name = argv[1]; 36 | baud_rate = atoi(argv[2]); 37 | dxl_id = atoi(argv[3]); 38 | new_baud_rate = atoi(argv[4]); 39 | } 40 | 41 | DynamixelWorkbench dxl_wb; 42 | 43 | const char *log; 44 | bool result = false; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeeded to init(%d)\n", baud_rate); 56 | 57 | uint16_t model_number = 0; 58 | result = dxl_wb.ping(dxl_id, &model_number, &log); 59 | if (result == false) 60 | { 61 | printf("%s\n", log); 62 | printf("Failed to ping\n"); 63 | 64 | return 0; 65 | } 66 | else 67 | { 68 | printf("Succeeded to ping\n"); 69 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 70 | } 71 | 72 | result = dxl_wb.changeBaudrate(dxl_id, new_baud_rate, &log); 73 | if (result == false) 74 | { 75 | printf("%s\n", log); 76 | return 0; 77 | } 78 | else 79 | { 80 | printf("%s\n", log); 81 | } 82 | 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/o_Find_Dynamixel.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | #define BAUDRATE_NUM 7 22 | 23 | int main(int argc, char *argv[]) 24 | { 25 | const char* port_name = "/dev/ttyUSB0"; 26 | 27 | if (argc < 2) 28 | { 29 | printf("Please set '-port_name' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | } 36 | 37 | DynamixelWorkbench dxl_wb; 38 | 39 | const char *log; 40 | bool result = false; 41 | 42 | uint8_t scanned_id[100]; 43 | uint8_t dxl_cnt = 0; 44 | 45 | uint32_t baudrate[BAUDRATE_NUM] = {9600, 57600, 115200, 1000000, 2000000, 3000000, 4000000}; 46 | uint8_t range = 253; 47 | 48 | uint8_t index = 0; 49 | 50 | while (index < BAUDRATE_NUM) 51 | { 52 | result = dxl_wb.init(port_name, baudrate[index], &log); 53 | if (result == false) 54 | { 55 | printf("%s\n", log); 56 | printf("Failed to init\n"); 57 | } 58 | else 59 | printf("Succeed to init(%d)\n", baudrate[index]); 60 | 61 | dxl_cnt = 0; 62 | for (uint8_t num = 0; num < 100; num++) scanned_id[num] = 0; 63 | 64 | printf("Wait for scan...\n"); 65 | result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log); 66 | if (result == false) 67 | { 68 | printf("%s\n", log); 69 | printf("Failed to scan\n"); 70 | } 71 | else 72 | { 73 | printf("Find %d Dynamixels\n", dxl_cnt); 74 | 75 | for (int cnt = 0; cnt < dxl_cnt; cnt++) 76 | printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); 77 | } 78 | 79 | index++; 80 | } 81 | 82 | return 0; 83 | } 84 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/h_Position.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.jointMode(dxl_id, 0, 0, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to change joint mode\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to change joint mode\n"); 78 | printf("Dynamixel is moving...\n"); 79 | 80 | for (int count = 0; count < 3; count++) 81 | { 82 | dxl_wb.goalPosition(dxl_id, (int32_t)0); 83 | sleep(3); 84 | 85 | dxl_wb.goalPosition(dxl_id, (int32_t)1023); 86 | sleep(3); 87 | } 88 | } 89 | 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/i_Velocity.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.wheelMode(dxl_id, 0, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to change wheel mode\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to change wheel mode\n"); 78 | printf("Dynamixel is moving...\n"); 79 | 80 | for (int count = 0; count < 3; count++) 81 | { 82 | dxl_wb.goalVelocity(dxl_id, (int32_t)-100); 83 | sleep(3); 84 | 85 | dxl_wb.goalVelocity(dxl_id, (int32_t)100); 86 | sleep(3); 87 | } 88 | 89 | dxl_wb.goalVelocity(dxl_id, (int32_t)0); 90 | } 91 | 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/k_Read_Write.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.itemWrite(dxl_id, "LED", 1, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to LED On\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to LED On\n"); 78 | } 79 | 80 | int32_t get_data = 0; 81 | result = dxl_wb.itemRead(dxl_id, "Present_Position", &get_data, &log); 82 | if (result == false) 83 | { 84 | printf("%s\n", log); 85 | printf("Failed to get present position\n"); 86 | } 87 | else 88 | { 89 | printf("Succeed to get present position(value : %d)\n", get_data); 90 | } 91 | 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/j_Current_Based_Position.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | 27 | if (argc < 4) 28 | { 29 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 30 | return 0; 31 | } 32 | else 33 | { 34 | port_name = argv[1]; 35 | baud_rate = atoi(argv[2]); 36 | dxl_id = atoi(argv[3]); 37 | } 38 | 39 | DynamixelWorkbench dxl_wb; 40 | 41 | const char *log; 42 | bool result = false; 43 | 44 | uint16_t model_number = 0; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeed to init(%d)\n", baud_rate); 56 | 57 | result = dxl_wb.ping(dxl_id, &model_number, &log); 58 | if (result == false) 59 | { 60 | printf("%s\n", log); 61 | printf("Failed to ping\n"); 62 | } 63 | else 64 | { 65 | printf("Succeed to ping\n"); 66 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 67 | } 68 | 69 | result = dxl_wb.currentBasedPositionMode(dxl_id, 30, &log); 70 | if (result == false) 71 | { 72 | printf("%s\n", log); 73 | printf("Failed to change current based position mode\n"); 74 | } 75 | else 76 | { 77 | printf("Succeed to change current based position mode\n"); 78 | printf("Dynamixel is moving...\n"); 79 | 80 | for (int count = 0; count < 3; count++) 81 | { 82 | dxl_wb.goalPosition(dxl_id, (int32_t)0); 83 | sleep(3); 84 | 85 | dxl_wb.goalPosition(dxl_id, (int32_t)2048); 86 | sleep(3); 87 | } 88 | } 89 | 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_tool.h: -------------------------------------------------------------------------------- 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 | #ifndef DYNAMIXEL_TOOL_H 20 | #define DYNAMIXEL_TOOL_H 21 | 22 | #include 23 | #include 24 | 25 | #include "dynamixel_item.h" 26 | 27 | class DynamixelTool 28 | { 29 | private: 30 | enum {DYNAMIXEL_BUFFER = 30}; 31 | uint8_t dxl_id_[DYNAMIXEL_BUFFER]; 32 | uint8_t dxl_cnt_; 33 | 34 | const char *model_name_; 35 | uint16_t model_number_; 36 | 37 | const ControlItem *control_table_; 38 | const ModelInfo *model_info_; 39 | 40 | uint16_t the_number_of_control_item_; 41 | 42 | public: 43 | DynamixelTool(); 44 | ~DynamixelTool(); 45 | 46 | void initTool(void); 47 | 48 | bool addTool(const char *model_name, uint8_t id, const char **log = NULL); 49 | bool addTool(uint16_t model_number, uint8_t id, const char **log = NULL); 50 | 51 | void addDXL(uint8_t id); 52 | 53 | const char *getModelName(void); 54 | uint16_t getModelNumber(void); 55 | 56 | const uint8_t* getID(void); 57 | uint8_t getDynamixelBuffer(void); 58 | uint8_t getDynamixelCount(void); 59 | 60 | float getRPM(void); 61 | 62 | int64_t getValueOfMinRadianPosition(void); 63 | int64_t getValueOfMaxRadianPosition(void); 64 | int64_t getValueOfZeroRadianPosition(void); 65 | 66 | float getMinRadian(void); 67 | float getMaxRadian(void); 68 | 69 | uint8_t getTheNumberOfControlItem(void); 70 | 71 | const ControlItem *getControlItem(const char *item_name, const char **log = NULL); 72 | const ControlItem *getControlTable(void); 73 | const ModelInfo *getModelInfo(void); 74 | 75 | private: 76 | bool setControlTable(const char *model_name, const char **log = NULL); 77 | bool setControlTable(uint16_t model_number, const char **log = NULL); 78 | 79 | bool setModelName(uint16_t model_number, const char **log = NULL); 80 | bool setModelNumber(const char *model_name, const char **log = NULL); 81 | }; 82 | #endif //DYNAMIXEL_TOOL_H 83 | -------------------------------------------------------------------------------- /.github/workflows/ros-ci.yml: -------------------------------------------------------------------------------- 1 | # The name of the workflow 2 | name: CI 3 | 4 | # Specifies the events that trigger the workflow 5 | on: 6 | push: 7 | branches: [ main, humble, jazzy] 8 | pull_request: 9 | branches: [ main, humble, jazzy] 10 | 11 | # Defines a set of jobs to be run as part of the workflow 12 | jobs: 13 | # The name of the job 14 | ROS_CI: 15 | runs-on: ubuntu-22.04 16 | strategy: 17 | fail-fast: false 18 | matrix: 19 | ros_distribution: 20 | - humble 21 | - jazzy 22 | - rolling 23 | include: 24 | # ROS 2 Humble Hawksbill 25 | - docker_image: ubuntu:jammy 26 | ros_distribution: humble 27 | ros_version: 2 28 | # ROS 2 Jazzy Jalisco 29 | - docker_image: ubuntu:noble 30 | ros_distribution: jazzy 31 | ros_version: 2 32 | # ROS 2 Rolling Ridley 33 | - docker_image: ubuntu:noble 34 | ros_distribution: rolling 35 | ros_version: 2 36 | container: 37 | image: ${{ matrix.docker_image }} 38 | steps: 39 | - name: Setup workspace 40 | run: mkdir -p ros_ws/src 41 | 42 | - name: Checkout code 43 | uses: actions/checkout@v4 44 | with: 45 | path: ros_ws/src 46 | 47 | - name: Setup ROS environment 48 | uses: ros-tooling/setup-ros@v0.7 49 | with: 50 | required-ros-distributions: ${{ matrix.ros_distribution }} 51 | 52 | - name: Check and Install ROS dependencies 53 | shell: bash 54 | run: | 55 | set -e 56 | source /opt/ros/${{ matrix.ros_distribution }}/setup.bash 57 | echo "--- Updating rosdep definitions ---" 58 | rosdep update 59 | echo "--- Installing system dependencies for ROS 2 ${{ matrix.ros_distribution }} ---" 60 | rosdep install --from-paths ros_ws/src --ignore-src -y -r --rosdistro ${{ matrix.ros_distribution }} 61 | echo "--- Performing rosdep check for ROS 2 ${{ matrix.ros_distribution }} ---" 62 | if rosdep check --from-paths ros_ws/src --ignore-src --rosdistro ${{ matrix.ros_distribution }}; then 63 | echo "--- rosdep check passed ---" 64 | else 65 | echo "--- rosdep check failed: Missing system dependencies or unresolvable keys. ---" 66 | exit 1 67 | fi 68 | 69 | - name: Build and Test 70 | uses: ros-tooling/action-ros-ci@v0.3 71 | with: 72 | target-ros2-distro: ${{ matrix.ros_distribution }} 73 | vcs-repo-file-url: "https://raw.githubusercontent.com/ROBOTIS-GIT/dynamixel-workbench/main/.github/workflows/dynamixel_workbench.repos" 74 | package-name: | 75 | dynamixel_workbench 76 | dynamixel_workbench_toolbox 77 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 3.5) 5 | project(dynamixel_workbench_toolbox) 6 | 7 | if(NOT CMAKE_CXX_STANDARD) 8 | set(CMAKE_CXX_STANDARD 14) 9 | endif() 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | if(MSVC) 16 | add_compile_definitions( 17 | _USE_MATH_DEFINES 18 | ) 19 | endif() 20 | 21 | ################################################################################ 22 | # Find and load build settings from external packages 23 | ################################################################################ 24 | find_package(ament_cmake REQUIRED) 25 | find_package(dynamixel_sdk REQUIRED) 26 | 27 | ################################################################################ 28 | # Declare ROS messages, services and actions 29 | ################################################################################ 30 | 31 | ################################################################################ 32 | # Build 33 | ################################################################################ 34 | 35 | set(LIB_NAME "dynamixel_workbench_toolbox") 36 | 37 | add_library(${LIB_NAME} SHARED 38 | src/${PROJECT_NAME}/dynamixel_item.cpp 39 | src/${PROJECT_NAME}/dynamixel_driver.cpp 40 | src/${PROJECT_NAME}/dynamixel_tool.cpp 41 | src/${PROJECT_NAME}/dynamixel_workbench.cpp 42 | ) 43 | target_include_directories(${LIB_NAME} PUBLIC 44 | $ 45 | $ 46 | ${dynamixel_sdk_INCLUDE_DIRS} 47 | ) 48 | 49 | target_link_libraries(${LIB_NAME} PUBLIC 50 | ${dynamixel_sdk_LIBRARIES} 51 | ) 52 | 53 | ################################################################################ 54 | # Install 55 | ################################################################################ 56 | install(TARGETS ${LIB_NAME} 57 | EXPORT export_${PROJECT_NAME} 58 | ARCHIVE DESTINATION lib 59 | LIBRARY DESTINATION lib 60 | RUNTIME DESTINATION bin/${PROJECT_NAME} 61 | ) 62 | 63 | install(DIRECTORY include/ 64 | DESTINATION include/${PROJECT_NAME} 65 | ) 66 | 67 | ################################################################################ 68 | # Test 69 | ################################################################################ 70 | 71 | ################################################################################ 72 | # Macro for ament package 73 | ################################################################################ 74 | ament_export_dependencies(dynamixel_sdk) 75 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 76 | ament_package() 77 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(dynamixel_workbench) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | include_directories( 7 | ../src 8 | ) 9 | 10 | add_library(dynamixel_workbench 11 | ../src/dynamixel_workbench_toolbox/dynamixel_item.cpp 12 | ../src/dynamixel_workbench_toolbox/dynamixel_driver.cpp 13 | ../src/dynamixel_workbench_toolbox/dynamixel_tool.cpp 14 | ../src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp 15 | ) 16 | 17 | if(APPLE) 18 | target_link_libraries(dynamixel_workbench LINK_PUBLIC "/usr/local/lib/libdxl_mac_cpp.dylib") 19 | elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "arm" OR CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64") 20 | target_link_libraries(dynamixel_workbench LINK_PUBLIC "/usr/local/lib/libdxl_sbc_cpp.so") 21 | else() 22 | target_link_libraries(dynamixel_workbench LINK_PUBLIC "/usr/local/lib/libdxl_x64_cpp.so") 23 | endif() 24 | 25 | add_executable(model_scan src/a_Model_Scan.cpp) 26 | target_link_libraries(model_scan LINK_PUBLIC dynamixel_workbench) 27 | 28 | add_executable(ping src/b_Ping.cpp) 29 | target_link_libraries(ping LINK_PUBLIC dynamixel_workbench) 30 | 31 | add_executable(id_change src/c_ID_Change.cpp) 32 | target_link_libraries(id_change LINK_PUBLIC dynamixel_workbench) 33 | 34 | add_executable(bps_change src/d_BPS_Change.cpp) 35 | target_link_libraries(bps_change LINK_PUBLIC dynamixel_workbench) 36 | 37 | add_executable(mode_change src/e_Mode_Change.cpp) 38 | target_link_libraries(mode_change LINK_PUBLIC dynamixel_workbench) 39 | 40 | add_executable(reboot src/f_Reboot.cpp) 41 | target_link_libraries(reboot LINK_PUBLIC dynamixel_workbench) 42 | 43 | add_executable(reset src/g_Reset.cpp) 44 | target_link_libraries(reset LINK_PUBLIC dynamixel_workbench) 45 | 46 | add_executable(position src/h_Position.cpp) 47 | target_link_libraries(position LINK_PUBLIC dynamixel_workbench) 48 | 49 | add_executable(velocity src/i_Velocity.cpp) 50 | target_link_libraries(velocity LINK_PUBLIC dynamixel_workbench) 51 | 52 | add_executable(current_based_position src/j_Current_Based_Position.cpp) 53 | target_link_libraries(current_based_position LINK_PUBLIC dynamixel_workbench) 54 | 55 | add_executable(read_write src/k_Read_Write.cpp) 56 | target_link_libraries(read_write LINK_PUBLIC dynamixel_workbench) 57 | 58 | add_executable(sync_write src/l_Sync_Write.cpp) 59 | target_link_libraries(sync_write LINK_PUBLIC dynamixel_workbench) 60 | 61 | add_executable(sync_read_write src/m_Sync_Read_Write.cpp) 62 | target_link_libraries(sync_read_write LINK_PUBLIC dynamixel_workbench) 63 | 64 | add_executable(bulk_read_write src/n_Bulk_Read_Write.cpp) 65 | target_link_libraries(bulk_read_write LINK_PUBLIC dynamixel_workbench) 66 | 67 | add_executable(find_dynamixel src/o_Find_Dynamixel.cpp) 68 | target_link_libraries(find_dynamixel LINK_PUBLIC dynamixel_workbench) 69 | 70 | add_executable(monitor src/p_Monitor.cpp) 71 | target_link_libraries(monitor LINK_PUBLIC dynamixel_workbench) 72 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/c_ID_Change.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2016 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | int new_dxl_id = 2; 27 | 28 | if (argc < 5) 29 | { 30 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id', '-new_dynamixel_id' arguments for connected Dynamixels\n"); 31 | return 0; 32 | } 33 | else 34 | { 35 | port_name = argv[1]; 36 | baud_rate = atoi(argv[2]); 37 | dxl_id = atoi(argv[3]); 38 | new_dxl_id = atoi(argv[4]); 39 | } 40 | 41 | DynamixelWorkbench dxl_wb; 42 | 43 | const char *log; 44 | bool result = false; 45 | 46 | result = dxl_wb.init(port_name, baud_rate, &log); 47 | if (result == false) 48 | { 49 | printf("%s\n", log); 50 | printf("Failed to init\n"); 51 | 52 | return 0; 53 | } 54 | else 55 | printf("Succeeded to init(%d)\n", baud_rate); 56 | 57 | uint16_t model_number = 0; 58 | result = dxl_wb.ping(dxl_id, &model_number, &log); 59 | if (result == false) 60 | { 61 | printf("%s\n", log); 62 | printf("Failed to ping\n"); 63 | 64 | return 0; 65 | } 66 | else 67 | { 68 | printf("Succeeded to ping\n"); 69 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 70 | } 71 | 72 | result = dxl_wb.changeID(dxl_id, new_dxl_id, &log); 73 | if (result == false) 74 | { 75 | printf("%s\n", log); 76 | return 0; 77 | } 78 | else 79 | { 80 | printf("%s\n", log); 81 | } 82 | 83 | uint8_t scanned_id[16]; 84 | uint8_t dxl_cnt = 0; 85 | uint8_t range = 100; 86 | 87 | printf("Wait for scan...\n"); 88 | result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log); 89 | if (result == false) 90 | { 91 | printf("%s\n", log); 92 | printf("Failed to scan\n"); 93 | } 94 | else 95 | { 96 | printf("Find %d Dynamixels\n", dxl_cnt); 97 | 98 | for (int cnt = 0; cnt < dxl_cnt; cnt++) 99 | printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); 100 | } 101 | } 102 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/l_Sync_Write.cpp: -------------------------------------------------------------------------------- 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 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 | 44 | DynamixelWorkbench dxl_wb; 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 | } 120 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_item.h: -------------------------------------------------------------------------------- 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), Ryan Shim */ 18 | 19 | #ifndef DYNAMIXEL_ITEM_H 20 | #define DYNAMIXEL_ITEM_H 21 | 22 | #include 23 | #include 24 | 25 | #define AX_12A 12 26 | #define AX_12W 300 27 | #define AX_18A 18 28 | 29 | #define RX_10 10 30 | #define RX_24F 24 31 | #define RX_28 28 32 | #define RX_64 64 33 | 34 | #define EX_106 107 35 | 36 | #define MX_12W 360 37 | #define MX_28 29 38 | #define MX_28_2 30 39 | #define MX_64 310 40 | #define MX_64_2 311 41 | #define MX_106 320 42 | #define MX_106_2 321 43 | 44 | #define XL_320 350 45 | 46 | #define XL330_M077 1190 47 | #define XL330_M288 1200 48 | 49 | #define XC330_M181 1230 50 | #define XC330_M288 1240 51 | #define XC330_T181 1210 52 | #define XC330_T288 1220 53 | 54 | #define XL430_W250 1060 55 | 56 | #define XL430_W250_2 1090 // 2XL 57 | #define XC430_W250_2 1160 // 2XC 58 | 59 | #define XC430_W150 1070 60 | #define XC430_W240 1080 61 | 62 | #define XM430_W210 1030 63 | #define XM430_W350 1020 64 | 65 | #define XM540_W150 1130 66 | #define XM540_W270 1120 67 | 68 | #define XH430_W210 1010 69 | #define XH430_W350 1000 70 | #define XH430_V210 1050 71 | #define XH430_V350 1040 72 | 73 | #define XH540_W150 1110 74 | #define XH540_W270 1100 75 | #define XH540_V150 1150 76 | #define XH540_V270 1140 77 | 78 | #define XW430_T200 1280 79 | #define XW430_T333 1270 80 | #define XW540_T260 1170 81 | #define XW540_T140 1180 82 | 83 | #define PRO_L42_10_S300_R 35072 84 | #define PRO_L54_30_S400_R 37928 85 | #define PRO_L54_30_S500_R 37896 86 | #define PRO_L54_50_S290_R 38176 87 | #define PRO_L54_50_S500_R 38152 88 | 89 | #define PRO_M42_10_S260_R 43288 90 | #define PRO_M54_40_S250_R 46096 91 | #define PRO_M54_60_S250_R 46352 92 | 93 | #define PRO_H42_20_S300_R 51200 94 | #define PRO_H54_100_S500_R 53768 95 | #define PRO_H54_200_S500_R 54024 96 | 97 | #define PRO_M42_10_S260_R_A 43289 98 | #define PRO_M54_40_S250_R_A 46097 99 | #define PRO_M54_60_S250_R_A 46353 100 | 101 | #define PRO_H42_20_S300_R_A 51201 102 | #define PRO_H54_100_S500_R_A 53769 103 | #define PRO_H54_200_S500_R_A 54025 104 | 105 | #define PRO_PLUS_M42P_010_S260_R 2100 106 | #define PRO_PLUS_M54P_040_S250_R 2110 107 | #define PRO_PLUS_M54P_060_S250_R 2120 108 | 109 | #define PRO_PLUS_H42P_020_S300_R 2000 110 | #define PRO_PLUS_H54P_100_S500_R 2010 111 | #define PRO_PLUS_H54P_200_S500_R 2020 112 | 113 | #define RH_P12_RN 35073 114 | #define RH_P12_RN_A 35074 115 | 116 | #define BYTE 1 117 | #define WORD 2 118 | #define DWORD 4 119 | 120 | typedef struct 121 | { 122 | const char *item_name; 123 | uint16_t address; 124 | uint8_t item_name_length; 125 | uint16_t data_length; 126 | } ControlItem; 127 | 128 | typedef struct 129 | { 130 | float rpm; 131 | 132 | int64_t value_of_min_radian_position; 133 | int64_t value_of_zero_radian_position; 134 | int64_t value_of_max_radian_position; 135 | 136 | float min_radian; 137 | float max_radian; 138 | } ModelInfo; 139 | 140 | // Public Functions 141 | namespace DynamixelItem 142 | { 143 | const ControlItem *getControlTable(uint16_t model_number); 144 | const ModelInfo *getModelInfo(uint16_t model_number); 145 | 146 | uint8_t getTheNumberOfControlItem(); 147 | } 148 | 149 | #endif //DYNAMIXEL_ITEM_H 150 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/m_Sync_Read_Write.cpp: -------------------------------------------------------------------------------- 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 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 | 44 | DynamixelWorkbench dxl_wb; 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 | result = dxl_wb.addSyncReadHandler(dxl_id[0], "Present_Position", &log); 94 | if (result == false) 95 | { 96 | printf("%s\n", log); 97 | printf("Failed to add sync read handler\n"); 98 | } 99 | 100 | int32_t goal_position[2] = {0, 1023}; 101 | int32_t present_position[2] = {0, 0}; 102 | 103 | const uint8_t handler_index = 0; 104 | 105 | while(1) 106 | { 107 | result = dxl_wb.syncWrite(handler_index, &goal_position[0], &log); 108 | if (result == false) 109 | { 110 | printf("%s\n", log); 111 | printf("Failed to sync write position\n"); 112 | } 113 | 114 | do 115 | { 116 | result = dxl_wb.syncRead(handler_index, &log); 117 | if (result == false) 118 | { 119 | printf("%s\n", log); 120 | printf("Failed to sync read position\n"); 121 | } 122 | 123 | result = dxl_wb.getSyncReadData(handler_index, &present_position[0], &log); 124 | if (result == false) 125 | { 126 | printf("%s\n", log); 127 | } 128 | else 129 | { 130 | printf("[ID %d]\tGoal Position : %d\tPresent Position : %d, [ID %d]\tGoal Position : %d\tPresent Position : %d\n" 131 | ,dxl_id[0], goal_position[0], present_position[0], dxl_id[1], goal_position[1], present_position[1]); 132 | } 133 | 134 | }while(abs(goal_position[0] - present_position[0]) > 15 && 135 | abs(goal_position[1] - present_position[1]) > 15); 136 | 137 | swap(goal_position); 138 | } 139 | 140 | return 0; 141 | } 142 | 143 | void swap(int32_t *array) 144 | { 145 | int32_t tmp = array[0]; 146 | array[0] = array[1]; 147 | array[1] = tmp; 148 | } 149 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/e_Mode_Change.cpp: -------------------------------------------------------------------------------- 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 20 | 21 | int main(int argc, char *argv[]) 22 | { 23 | const char* port_name = "/dev/ttyUSB0"; 24 | int baud_rate = 57600; 25 | int dxl_id = 1; 26 | int mode = 0; 27 | 28 | if (argc < 4) 29 | { 30 | printf("Please set '-port_name', '-baud_rate', '-dynamixel id' arguments for connected Dynamixels\n"); 31 | return 0; 32 | } 33 | else 34 | { 35 | port_name = argv[1]; 36 | baud_rate = atoi(argv[2]); 37 | dxl_id = atoi(argv[3]); 38 | } 39 | 40 | DynamixelWorkbench dxl_wb; 41 | 42 | const char *log; 43 | bool result = false; 44 | 45 | uint16_t model_number = 0; 46 | 47 | result = dxl_wb.init(port_name, baud_rate, &log); 48 | if (result == false) 49 | { 50 | printf("%s\n", log); 51 | printf("Failed to init\n"); 52 | 53 | return 0; 54 | } 55 | else 56 | printf("Succeeded to init(%d)\n", baud_rate); 57 | 58 | result = dxl_wb.ping(dxl_id, &model_number, &log); 59 | if (result == false) 60 | { 61 | printf("%s\n", log); 62 | printf("Failed to ping\n"); 63 | } 64 | else 65 | { 66 | printf("Succeeded to ping\n"); 67 | printf("id : %d, model_number : %d\n", dxl_id, model_number); 68 | } 69 | 70 | printf("---> \n"); 71 | printf("Please insert the right number from 0 to 5 to set control mode \n"); 72 | printf("0 - current control mode\n"); 73 | printf("1 - velocity control mode\n"); 74 | printf("2 - position control mode\n"); 75 | printf("3 - extended position control mode\n"); 76 | printf("4 - current based position control mode\n"); 77 | printf("5 - pwm control mode\n"); 78 | printf("<--- \n\n"); 79 | 80 | scanf("%d", &mode); 81 | 82 | switch (mode) 83 | { 84 | case 0: 85 | result = dxl_wb.setCurrentControlMode(dxl_id, &log); 86 | if (result == false) 87 | { 88 | printf("%s\n", log); 89 | printf("Failed to set mode\n"); 90 | } 91 | else 92 | { 93 | printf("Succeeded to set mode\n"); 94 | } 95 | break; 96 | 97 | case 1: 98 | result = dxl_wb.setVelocityControlMode(dxl_id, &log); 99 | if (result == false) 100 | { 101 | printf("%s\n", log); 102 | printf("Failed to set mode\n"); 103 | } 104 | else 105 | { 106 | printf("Succeeded to set mode\n"); 107 | } 108 | break; 109 | 110 | case 2: 111 | result = dxl_wb.setPositionControlMode(dxl_id, &log); 112 | if (result == false) 113 | { 114 | printf("%s\n", log); 115 | printf("Failed to set mode\n"); 116 | } 117 | else 118 | { 119 | printf("Succeeded to set mode\n"); 120 | } 121 | break; 122 | 123 | case 3: 124 | result = dxl_wb.setExtendedPositionControlMode(dxl_id, &log); 125 | if (result == false) 126 | { 127 | printf("%s\n", log); 128 | printf("Failed to set mode\n"); 129 | } 130 | else 131 | { 132 | printf("Succeeded to set mode\n"); 133 | } 134 | break; 135 | 136 | case 4: 137 | result = dxl_wb.setCurrentBasedPositionControlMode(dxl_id, &log); 138 | if (result == false) 139 | { 140 | printf("%s\n", log); 141 | printf("Failed to set mode\n"); 142 | } 143 | else 144 | { 145 | printf("Succeeded to set mode\n"); 146 | } 147 | break; 148 | 149 | case 5: 150 | result = dxl_wb.setPWMControlMode(dxl_id, &log); 151 | if (result == false) 152 | { 153 | printf("%s\n", log); 154 | printf("Failed to set mode\n"); 155 | } 156 | else 157 | { 158 | printf("Succeeded to set mode\n"); 159 | } 160 | break; 161 | 162 | default: 163 | result = dxl_wb.setPositionControlMode(dxl_id, &log); 164 | if (result == false) 165 | { 166 | printf("%s\n", log); 167 | printf("Failed to set mode\n"); 168 | } 169 | else 170 | { 171 | printf("Succeeded to set mode\n"); 172 | } 173 | break; 174 | } 175 | 176 | return 0; 177 | } 178 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_workbench.h: -------------------------------------------------------------------------------- 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 | #ifndef DYNAMIXEL_WORKBENCH_H_ 20 | #define DYNAMIXEL_WORKBENCH_H_ 21 | 22 | #include "dynamixel_driver.h" 23 | 24 | class DynamixelWorkbench : public DynamixelDriver 25 | { 26 | public: 27 | DynamixelWorkbench(); 28 | ~DynamixelWorkbench(); 29 | 30 | bool torque(uint8_t id, int32_t onoff, const char **log = NULL); 31 | bool torqueOn(uint8_t id, const char **log = NULL); 32 | bool torqueOff(uint8_t id, const char **log = NULL); 33 | 34 | bool changeID(uint8_t id, uint8_t new_id, const char **log = NULL); 35 | bool changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log = NULL); 36 | bool changeProtocolVersion(uint8_t id, uint8_t version, const char **log = NULL); 37 | 38 | bool itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); 39 | bool itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log = NULL); 40 | 41 | bool led(uint8_t id, int32_t onoff, const char **log = NULL); 42 | bool ledOn(uint8_t id, const char **log = NULL); 43 | bool ledOff(uint8_t id, const char **log = NULL); 44 | 45 | bool setNormalDirection(uint8_t id, const char **log = NULL); 46 | bool setReverseDirection(uint8_t id, const char **log = NULL); 47 | 48 | bool setVelocityBasedProfile(uint8_t id, const char **log = NULL); 49 | bool setTimeBasedProfile(uint8_t id, const char **log = NULL); 50 | 51 | bool setSecondaryID(uint8_t id, uint8_t secondary_id, const char **log = NULL); 52 | 53 | bool setCurrentControlMode(uint8_t id, const char **log = NULL); 54 | bool setTorqueControlMode(uint8_t id, const char **log = NULL); 55 | bool setVelocityControlMode(uint8_t id, const char **log = NULL); 56 | bool setPositionControlMode(uint8_t id, const char **log = NULL); 57 | bool setExtendedPositionControlMode(uint8_t id, const char **log = NULL); 58 | bool setMultiTurnControlMode(uint8_t id, const char **log = NULL); 59 | bool setCurrentBasedPositionControlMode(uint8_t id, const char **log = NULL); 60 | bool setPWMControlMode(uint8_t id, const char **log = NULL); 61 | 62 | bool setOperatingMode(uint8_t id, uint8_t index, const char **log = NULL); 63 | 64 | bool jointMode(uint8_t id, int32_t velocity = 0, int32_t acceleration = 0, const char **log = NULL); 65 | bool wheelMode(uint8_t id, int32_t acceleration = 0, const char **log = NULL); 66 | bool currentBasedPositionMode(uint8_t id, int32_t current = 0, const char **log = NULL); 67 | 68 | bool goalPosition(uint8_t id, int value, const char **log = NULL); //keep compatibility with older codes 69 | // bool goalPosition(uint8_t id, int32_t value, const char **log = NULL); 70 | bool goalPosition(uint8_t id, float radian, const char **log = NULL); 71 | 72 | bool goalSpeed(uint8_t id, int value, const char **log = NULL); //keep compatibility with older codes 73 | bool goalVelocity(uint8_t id, int value, const char **log = NULL); //keep compatibility with older codes 74 | // bool goalVelocity(uint8_t id, int32_t value, const char **log = NULL); 75 | bool goalVelocity(uint8_t id, float velocity, const char **log = NULL); 76 | 77 | bool getPresentPositionData(uint8_t id, int32_t* data, const char **log = NULL); 78 | bool getRadian(uint8_t id, float* radian, const char **log = NULL); 79 | 80 | bool getPresentVelocityData(uint8_t id, int32_t* data, const char **log = NULL); 81 | bool getVelocity(uint8_t id, float* velocity, const char **log = NULL); 82 | 83 | int32_t convertRadian2Value(uint8_t id, float radian); 84 | float convertValue2Radian(uint8_t id, int32_t value); 85 | 86 | int32_t convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian, float min_radian); 87 | float convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian, float min_radian); 88 | 89 | int32_t convertVelocity2Value(uint8_t id, float velocity); 90 | float convertValue2Velocity(uint8_t id, int32_t value); 91 | 92 | int16_t convertCurrent2Value(uint8_t id, float current); 93 | int16_t convertCurrent2Value(float current); 94 | float convertValue2Current(uint8_t id, int16_t value); 95 | float convertValue2Current(int16_t value); 96 | 97 | float convertValue2Load(int16_t value); 98 | }; 99 | 100 | #endif /*DYNAMIXEL_WORKBENCH_H_*/ 101 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/n_Bulk_Read_Write.cpp: -------------------------------------------------------------------------------- 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 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 | 44 | DynamixelWorkbench dxl_wb; 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 | } 201 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_driver.h: -------------------------------------------------------------------------------- 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 | #ifndef DYNAMIXEL_DRIVER_H 20 | #define DYNAMIXEL_DRIVER_H 21 | 22 | #include "dynamixel_tool.h" 23 | 24 | #if defined(__OPENCR__) || defined(__OPENCM904__) 25 | #include 26 | #include 27 | #elif defined(__linux__) || defined(__APPLE__) 28 | #include "unistd.h" 29 | #include "dynamixel_sdk/dynamixel_sdk.h" 30 | #endif 31 | 32 | #define MAX_DXL_SERIES_NUM 5 33 | #define MAX_HANDLER_NUM 5 34 | #define MAX_BULK_PARAMETER 20 35 | 36 | typedef struct 37 | { 38 | const ControlItem *control_item; 39 | dynamixel::GroupSyncWrite *groupSyncWrite; 40 | } SyncWriteHandler; 41 | 42 | typedef struct 43 | { 44 | const ControlItem *control_item; 45 | dynamixel::GroupSyncRead *groupSyncRead; 46 | } SyncReadHandler; 47 | 48 | typedef struct 49 | { 50 | uint8_t id; 51 | uint16_t address; 52 | uint16_t data_length; 53 | } BulkParameter; 54 | 55 | typedef struct 56 | { 57 | int dxl_comm_result; 58 | bool dxl_addparam_result; 59 | bool dxl_getdata_result; 60 | uint8_t dxl_error; 61 | } ErrorFromSDK; 62 | 63 | class DynamixelDriver 64 | { 65 | private: 66 | dynamixel::PortHandler *portHandler_; 67 | dynamixel::PacketHandler *packetHandler_; 68 | 69 | SyncWriteHandler syncWriteHandler_[MAX_HANDLER_NUM]; 70 | SyncReadHandler syncReadHandler_[MAX_HANDLER_NUM]; 71 | 72 | dynamixel::GroupBulkRead *groupBulkRead_; 73 | dynamixel::GroupBulkWrite *groupBulkWrite_; 74 | BulkParameter bulk_read_param_[MAX_BULK_PARAMETER]; 75 | 76 | DynamixelTool tools_[MAX_DXL_SERIES_NUM]; 77 | 78 | uint8_t tools_cnt_; 79 | uint8_t sync_write_handler_cnt_; 80 | uint8_t sync_read_handler_cnt_; 81 | uint8_t bulk_read_parameter_cnt_; 82 | 83 | public: 84 | DynamixelDriver(); 85 | ~DynamixelDriver(); 86 | 87 | bool init(const char* device_name = "/dev/ttyUSB0", 88 | uint32_t baud_rate = 57600, 89 | const char **log = NULL); 90 | 91 | bool begin(const char* device_name = "/dev/ttyUSB0", 92 | uint32_t baud_rate = 57600, 93 | const char **log = NULL); 94 | 95 | bool setPortHandler(const char *device_name, const char **log = NULL); 96 | bool setBaudrate(uint32_t baud_rate, const char **log = NULL); 97 | bool setPacketHandler(float protocol_version, const char **log = NULL); 98 | 99 | float getProtocolVersion(void); 100 | uint32_t getBaudrate(void); 101 | 102 | const char * getModelName(uint8_t id, const char **log = NULL); 103 | uint16_t getModelNumber(uint8_t id, const char **log = NULL); 104 | const ControlItem *getControlTable(uint8_t id, const char **log = NULL); 105 | const ControlItem *getItemInfo(uint8_t id, const char *item_name, const char **log = NULL); 106 | uint8_t getTheNumberOfControlItem(uint8_t id, const char **log = NULL); 107 | const ModelInfo* getModelInfo(uint8_t id, const char **log = NULL); 108 | 109 | uint8_t getTheNumberOfSyncWriteHandler(void); 110 | uint8_t getTheNumberOfSyncReadHandler(void); 111 | uint8_t getTheNumberOfBulkReadParam(void); 112 | 113 | bool scan(uint8_t *get_id, 114 | uint8_t *get_the_number_of_id, 115 | uint8_t range = 253, 116 | const char **log = NULL); 117 | 118 | bool scan(uint8_t *get_id, 119 | uint8_t *get_the_number_of_id, 120 | uint8_t start_number, 121 | uint8_t end_number, 122 | const char **log = NULL); 123 | 124 | bool ping(uint8_t id, 125 | uint16_t *get_model_number, 126 | const char **log = NULL); 127 | 128 | bool ping(uint8_t id, 129 | const char **log = NULL); 130 | 131 | bool clearMultiTurn(uint8_t id, const char **log = NULL); 132 | 133 | bool reboot(uint8_t id, const char **log = NULL); 134 | bool reset(uint8_t id, const char **log = NULL); 135 | 136 | bool writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t* data, const char **log = NULL); 137 | bool writeRegister(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); 138 | 139 | bool writeOnlyRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log = NULL); 140 | bool writeOnlyRegister(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); 141 | 142 | bool readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t *data, const char **log = NULL); 143 | bool readRegister(uint8_t id, const char *item_name, int32_t *data, const char **log = NULL); 144 | 145 | void getParam(int32_t data, uint8_t *param); 146 | 147 | bool addSyncWriteHandler(uint16_t address, uint16_t length, const char **log = NULL); 148 | bool addSyncWriteHandler(uint8_t id, const char *item_name, const char **log = NULL); 149 | 150 | bool syncWrite(uint8_t index, int32_t *data, const char **log = NULL); 151 | bool syncWrite(uint8_t index, uint8_t *id, uint8_t id_num, int32_t *data, uint8_t data_num_for_each_id, const char **log = NULL); 152 | 153 | bool addSyncReadHandler(uint16_t address, uint16_t length, const char **log = NULL); 154 | bool addSyncReadHandler(uint8_t id, const char *item_name, const char **log = NULL); 155 | 156 | bool syncRead(uint8_t index, const char **log = NULL); 157 | bool syncRead(uint8_t index, uint8_t *id, uint8_t id_num, const char **log = NULL); 158 | 159 | bool getSyncReadData(uint8_t index, int32_t *data, const char **log = NULL); 160 | bool getSyncReadData(uint8_t index, uint8_t *id, uint8_t id_num, int32_t *data, const char **log = NULL); 161 | bool getSyncReadData(uint8_t index, uint8_t *id, uint8_t id_num, uint16_t address, uint16_t length, int32_t *data, const char **log = NULL); 162 | 163 | bool initBulkWrite(const char **log = NULL); 164 | 165 | bool addBulkWriteParam(uint8_t id, uint16_t address, uint16_t length, int32_t data, const char **log = NULL); 166 | bool addBulkWriteParam(uint8_t id, const char *item_name, int32_t data, const char **log = NULL); 167 | 168 | bool bulkWrite(const char **log = NULL); 169 | 170 | bool initBulkRead(const char **log = NULL); 171 | 172 | bool addBulkReadParam(uint8_t id, uint16_t address, uint16_t length, const char **log = NULL); 173 | bool addBulkReadParam(uint8_t id, const char *item_name, const char **log = NULL); 174 | 175 | bool bulkRead(const char **log = NULL); 176 | 177 | bool getBulkReadData(int32_t *data, const char **log = NULL); 178 | bool getBulkReadData(uint8_t *id, uint8_t id_num, uint16_t *address, uint16_t *length, int32_t *data, const char **log = NULL); 179 | 180 | bool clearBulkReadParam(void); 181 | 182 | private: 183 | void initTools(void); 184 | bool setTool(uint16_t model_number, uint8_t id, const char **log = NULL); 185 | uint8_t getTool(uint8_t id, const char **log = NULL); 186 | }; 187 | 188 | #endif //DYNAMIXEL_DRIVER_H 189 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package dynamixel_workbench_toolbox 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.5 (2025-06-04) 6 | ------------------ 7 | * Remove unused rclcpp dependency 8 | * Remove ament_target_dependencies (deprecated in ROS 2 kilted) 9 | * Contributors: ijnek 10 | 11 | 2.2.4 (2025-03-14) 12 | ------------------ 13 | * Fixed the issue where the Workbench example was not building on SBC by adding the ARM option. 14 | * Added the CI for ROS 2 rolling, jazzy and Humble. 15 | * Contributors: Wonho Yun 16 | 17 | 2.2.3 (2022-10-06) 18 | ------------------ 19 | * ROS2 release (Foxy, Galactic, Humble) 20 | * fix variable length warning (#364) 21 | * Contributors: Kenji Brameld, Will Son 22 | 23 | 2.2.2 (2022-01-25) 24 | ------------------ 25 | * ROS2 package update only 26 | * Support 2XC430 on ROS2 (#342) 27 | * Support X330 series, XW series on ROS2 (#350) 28 | * New features(Startup Config, Backup Ready) updated in the Control Table Item list 29 | Start Config : https://emanual.robotis.com/docs/en/dxl/x/xm430-w210/#startup-configuration 30 | Backup Ready : https://emanual.robotis.com/docs/en/dxl/x/xm430-w210/#backup-ready 31 | * Contributors: Richard Osterloh, Yoshito Okada, Will Son 32 | 33 | 2.1.0 (2019-06-04) 34 | ------------------ 35 | * Removed single_manager and single_manager_gui 36 | * Added XH540, PRO (A Firmware), RH_P12_RN, RH_P12_RN (A Firmware) 37 | * Contributors: Ryan Shim 38 | 39 | 2.0.0 (2019-01-04) 40 | ------------------ 41 | * added YAML configuration for Dynamixel 42 | * supported position control mode, current based position control mode through Joint Trajectory Message 43 | * supported velocity control mode through Twist Message 44 | * reduced memory usage(Contribute by @KurtE) 45 | * updated API for sync, bulk instruction 46 | * updated control period (default is 10ms) 47 | * supported Linux, macos 48 | * supported Pro+ 49 | * supported DynamixelSDK(after v.3.6.2) 50 | * supported OpenManipulator 51 | * Contributors: Darby Lim, KurtE, Pyo 52 | 53 | 1.0.0 (2018-07-20) 54 | ------------------ 55 | * upgraded read time #162 56 | * added Dynamixel PRO information #162 57 | * added readRegister function 58 | * update dxl pro info 59 | * update proInfo func 60 | * modified max radian position 61 | * Contributors: Darby Lim, Pyo, Taehun Lim 62 | 63 | 0.3.1 (2018-06-04) 64 | ------------------ 65 | * None 66 | 67 | 0.3.0 (2018-06-01) 68 | ------------------ 69 | * added getProtocolVersion() 70 | * changed max_dxl_deries_num 71 | * merged pull request `#152 `_ `#151 `_ `#149 `_ `#132 `_ 72 | * Contributors: Darby Lim, Pyo 73 | 74 | 0.2.4 (2018-03-20) 75 | ------------------ 76 | * changed package.xml to format v2 77 | * Contributors: Pyo 78 | 79 | 0.2.3 (2018-03-09) 80 | ------------------ 81 | * None 82 | 83 | 0.2.2 (2018-02-28) 84 | ------------------ 85 | * modified the CI configurations (`#117 `_) 86 | * modified the CMakeLists.txt to fix wrong path (`ros/rosdistro#17019 `_) 87 | * Contributors: Pyo 88 | 89 | 0.2.1 (2018-02-22) 90 | ------------------ 91 | * modified the CMakeLists.txt to fix wrong path 92 | * Contributors: Pyo 93 | 94 | 0.2.0 (2018-02-19) 95 | ------------------ 96 | * added conver function to PRO user 97 | * added dxl_info_cnt init function 98 | * added compatibility for different protocol 99 | * added static 100 | * added convert function 101 | * added baudrate sorting 102 | * added all dynamixel series 103 | * added RX-10 104 | * added millis 105 | * added init dynamixel example 106 | * added setting for packet handler 107 | * added monitor example 108 | * added item 109 | * added dynamixel_item 110 | * added toolbox_ros and modified arduino path 111 | * added linux build and example 112 | * added begin and getprotocolversion function 113 | * modified linux version 114 | * modified description 115 | * modified model_info 116 | * modified variable range 117 | * modified setTools function 118 | * modified sync function 119 | * modified merge conflict 120 | * modified variable name 121 | * modified reset function 122 | * modified function name and return variable name 123 | * modified name of return var 124 | * modified item name 125 | * modified reset function 126 | * modified item name (added underscore) 127 | * modified function name 128 | * modified function for ROS depend 129 | * modified function to make space 130 | * modified begin function to reduce storage space 131 | * modified MX (2.0) protocol setting bug 132 | * modified example 133 | * modified sync and bulk comm 134 | * modified lib 135 | * modified begin 136 | * modified variable 137 | * modified begin function 138 | * modified joint and wheel mode 139 | * modified variable name 140 | * modified begin function 141 | * modified set function 142 | * modified dynamixel item 143 | * modified scan function 144 | * modified folder tree 145 | * modified dynamixel_tool 146 | * modified toolbox structure 147 | * modified .device and modified funtion for opencm and opencr 148 | * modified ifdef 149 | * modified get file 150 | * modified arduino version 151 | * modified get device in arduino 152 | * fixed reset bug 153 | * deleted dead code 154 | * deleted empty space 155 | * deleted xl define 156 | * deleted debug code and update ping func 157 | * test OpenCM 158 | * Contributors: Darby Lim, Yoonseok Pyo 159 | 160 | 0.1.9 (2017-11-03) 161 | ------------------ 162 | * modified dependency 163 | * Contributors: Darby Lim 164 | 165 | 0.1.8 (2017-11-01) 166 | ------------------ 167 | * None 168 | 169 | 0.1.7 (2017-10-30) 170 | ------------------ 171 | * added rospy for the issue https://github.com/ROBOTIS-GIT/dynamixel-workbench/issues/77 172 | * Contributors: Darby Lim 173 | 174 | 0.1.6 (2017-08-09) 175 | ------------------ 176 | * bug fixed 177 | * updated error msg 178 | * updated get model path 179 | * updated Dynamixel PRO 180 | * updated Dynamixel XL, XM and XH 181 | * updated annotation 182 | * updated multi driver 183 | * updated address name 184 | * updated msg name 185 | * modified launch files 186 | * modified variable 187 | * modified file location 188 | * added sync read 189 | * added multi read function 190 | * added multi_driver 191 | * changed BSD license to Apache 2.0 license 192 | * Contributors: Darby Lim 193 | 194 | 0.1.5 (2017-05-23) 195 | ------------------ 196 | * modified the cmake of toolbox 197 | * Contributors: Darby Lim 198 | 199 | 0.1.4 (2017-04-24) 200 | ------------------ 201 | * toolbox bug fixed 202 | * added dynamixel new model: XL430_W250 203 | * added dynamixel new model: XH 204 | * renamed current controller -> torque controller 205 | * Contributors: Darby Lim 206 | 207 | 0.1.3 (2016-11-29) 208 | ------------------ 209 | * modifiy folder path 210 | * add drive_mode in XM series 211 | * Contributors: Darby Lim 212 | 213 | 0.1.2 (2016-10-31) 214 | ------------------ 215 | * modify beta test feedback 216 | * Contributors: Darby Lim 217 | 218 | 0.1.1 (2016-10-21) 219 | ------------------ 220 | * Revert "add baudrate combobox and modify velocity controller" 221 | This reverts commit f4f83761d687c40660a2c864aa4fcbebe1df4ea4. 222 | * add baudrate combobox and modify velocity controller 223 | * Contributors: Darby Lim 224 | 225 | 0.1.0 (2016-09-23) 226 | ------------------------- 227 | * modified the package information for release 228 | * edit cmake and xml files 229 | * modify message 230 | * add multiport controller and torque controller 231 | * add position, velocity controller and pan-tilt, wheel tutorials 232 | * add GUI package 233 | * add pan tilt and wheel node in tutorial package 234 | * add tutorial package 235 | * add position, velocity, torque control package and change workbench_tool to workbench_toolbox 236 | * Contributors: Darby Lim, Pyo 237 | -------------------------------------------------------------------------------- /dynamixel_workbench/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package dynamixel_workbench 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.5 (2025-06-04) 6 | ------------------ 7 | * Remove unused rclcpp dependency 8 | * Remove ament_target_dependencies (deprecated in ROS 2 kilted) 9 | * Contributors: ijnek 10 | 11 | 2.2.4 (2025-03-14) 12 | ------------------ 13 | * Fixed the issue where the Workbench example was not building on SBC by adding the ARM option. 14 | * Added the CI for ROS 2 rolling, jazzy and Humble. 15 | * Contributors: Wonho Yun 16 | 17 | 2.2.3 (2022-10-06) 18 | ------------------ 19 | * ROS2 release (Foxy, Galactic, Humble) 20 | * fix variable length warning (#364) 21 | * Contributors: Kenji Brameld, Will Son 22 | 23 | 2.2.2 (2022-01-25) 24 | ------------------ 25 | * ROS2 package update only 26 | * Support 2XC430 on ROS2 (#342) 27 | * Support X330 series, XW series on ROS2 (#350) 28 | * New features(Startup Config, Backup Ready) updated in the Control Table Item list 29 | Start Config : https://emanual.robotis.com/docs/en/dxl/x/xm430-w210/#startup-configuration 30 | Backup Ready : https://emanual.robotis.com/docs/en/dxl/x/xm430-w210/#backup-ready 31 | * Contributors: Richard Osterloh, Yoshito Okada, Will Son 32 | 33 | 2.1.0 (2019-06-04) 34 | ------------------ 35 | * Removed single_manager and single_manager_gui 36 | * Added XH540, PRO (A Firmware), RH_P12_RN, RH_P12_RN (A Firmware) 37 | * Contributors: Ryan Shim 38 | 39 | 2.0.0 (2019-01-04) 40 | ------------------ 41 | * added YAML configuration for Dynamixel 42 | * supported position control mode, current based position control mode through Joint Trajectory Message 43 | * supported velocity control mode through Twist Message 44 | * reduced memory usage(Contribute by @KurtE) 45 | * updated API for sync, bulk instruction 46 | * updated control period (default is 10ms) 47 | * supported Linux, macos 48 | * supported Pro+ 49 | * supported DynamixelSDK(after v.3.6.2) 50 | * supported OpenManipulator 51 | * Contributors: Darby Lim, KurtE, Pyo 52 | 53 | 1.0.0 (2018-07-20) 54 | ------------------ 55 | * upgraded read time #162 56 | * added Dynamixel PRO information #162 57 | * added proext function and amended readRegister 58 | * added mutex for log thread 59 | * added timer for data log 60 | * added PROext header 61 | * added readRegister function 62 | * updated dxl ext function 63 | * update dxl pro info 64 | * update proInfo func 65 | * modified max radian position 66 | * Contributors: Darby Lim, Pyo 67 | 68 | 0.3.1 (2018-06-04) 69 | ------------------ 70 | * updated Qt5 and delete Qt4 config 71 | * deleted rqt_plugin header 72 | * merged pull request `#154 `_ `#153 `_ 73 | * Contributors: Darby Lim 74 | 75 | 0.3.0 (2018-06-01) 76 | ------------------ 77 | * added cmd_vel topics, joint_state topics, msgs 78 | * added getProtocolVersion() 79 | * changed max_dxl_deries_num 80 | * changed compile options for qt5 81 | * deleted unused code 82 | * merged pull request `#152 `_ `#151 `_ `#149 `_ `#132 `_ 83 | * Contributors: Darby Lim, Pyo 84 | 85 | 0.2.4 (2018-03-20) 86 | ------------------ 87 | * changed package.xml to format v2 88 | * Contributors: Pyo 89 | 90 | 0.2.3 (2018-03-09) 91 | ------------------ 92 | * modified dialog rotate direction 93 | * added dynamixel_sdk lib 94 | * added debug code 95 | * deleted anotation 96 | * Contributors: Darby Lim 97 | 98 | 0.2.2 (2018-02-28) 99 | ------------------ 100 | * modified the CI configurations (`#117 `_) 101 | * modified the CMakeLists.txt to fix wrong path (`ros/rosdistro#17019 `_) 102 | * Contributors: Pyo 103 | 104 | 0.2.1 (2018-02-22) 105 | ------------------ 106 | * modified the CMakeLists.txt to fix wrong path 107 | * Contributors: Pyo 108 | 109 | 0.2.0 (2018-02-19) 110 | ------------------ 111 | * added conver function to PRO user 112 | * added dxl_info_cnt init function 113 | * added compatibility for different protocol 114 | * added static 115 | * added convert function 116 | * added baudrate sorting 117 | * added all dynamixel series 118 | * added RX-10 119 | * added millis 120 | * added init dynamixel example 121 | * added setting for packet handler 122 | * added monitor example 123 | * added item 124 | * added dynamixel_item 125 | * added toolbox_ros and modified arduino path 126 | * added linux build and example 127 | * added begin and getprotocolversion function 128 | * modified linux version 129 | * modified description 130 | * modified model_info 131 | * modified variable range 132 | * modified setTools function 133 | * modified sync function 134 | * modified merge conflict 135 | * modified variable name 136 | * modified reset function 137 | * modified function name and return variable name 138 | * modified name of return var 139 | * modified item name 140 | * modified reset function 141 | * modified item name (added underscore) 142 | * modified function name 143 | * modified function for ROS depend 144 | * modified function to make space 145 | * modified begin function to reduce storage space 146 | * modified MX (2.0) protocol setting bug 147 | * modified example 148 | * modified sync and bulk comm 149 | * modified lib 150 | * modified begin 151 | * modified variable 152 | * modified begin function 153 | * modified joint and wheel mode 154 | * modified variable name 155 | * modified begin function 156 | * modified set function 157 | * modified dynamixel item 158 | * modified scan function 159 | * modified folder tree 160 | * modified dynamixel_tool 161 | * modified toolbox structure 162 | * modified .device and modified funtion for opencm and opencr 163 | * modified ifdef 164 | * modified get file 165 | * modified arduino version 166 | * modified get device in arduino 167 | * fixed reset bug 168 | * deleted dead code 169 | * deleted empty space 170 | * deleted xl define 171 | * deleted debug code and update ping func 172 | * test OpenCM 173 | * Contributors: Darby Lim, Karl D. Hansen, Yoonseok Pyo 174 | 175 | 0.1.9 (2017-11-03) 176 | ------------------ 177 | * deleted libqt4 (single_manager_gui) 178 | * modified dependency (controller, single_manager, toolbox) 179 | * Contributors: Darby Lim 180 | 181 | 0.1.8 (2017-11-01) 182 | ------------------ 183 | * deleted install inst 184 | * Contributors: Darby Lim 185 | 186 | 0.1.7 (2017-10-30) 187 | ------------------ 188 | * added rospy for the issue https://github.com/ROBOTIS-GIT/dynamixel-workbench/issues/77 189 | * Contributors: Darby Lim 190 | 191 | 0.1.6 (2017-08-09) 192 | ------------------ 193 | * bug fixed 194 | * updated error msg 195 | * updated get model path 196 | * updated Dynamixel PRO 197 | * updated Dynamixel XL, XM and XH 198 | * updated annotation 199 | * updated multi driver 200 | * updated address name 201 | * updated msg name 202 | * modified launch files 203 | * modified variable 204 | * modified file location 205 | * added sync read 206 | * added multi read function 207 | * added multi_driver 208 | * changed BSD license to Apache 2.0 license 209 | * Contributors: Darby Lim 210 | 211 | 0.1.5 (2017-05-23) 212 | ------------------ 213 | * modified the cmake of toolbox 214 | * Contributors: Darby Lim 215 | 216 | 0.1.4 (2017-04-24) 217 | ------------------ 218 | * toolbox bug fixed 219 | * added dynamixel new model: XL430_W250 220 | * added dynamixel new model: XH 221 | * renamed current controller -> torque controller 222 | * Contributors: Darby Lim 223 | 224 | 0.1.3 (2016-11-29) 225 | ------------------ 226 | * update torque controller 227 | * add control parameters 228 | * modified ros nodehandle 229 | * Contributors: Darby Lim 230 | 231 | 0.1.2 (2016-10-31) 232 | ------------------ 233 | * add stop sign in velocity controller 234 | * modify beta test feedback 235 | * Contributors: Darby Lim 236 | 237 | 0.1.1 (2016-10-21) 238 | ------------------ 239 | * Revert "add baudrate combobox and modify velocity controller" 240 | This reverts commit f4f83761d687c40660a2c864aa4fcbebe1df4ea4. 241 | * add baudrate combobox and modify velocity controller 242 | * Contributors: Darby Lim 243 | 244 | 0.1.0 (2016-09-23) 245 | ------------------------- 246 | * modified the package information for release 247 | * edit cmake and xml files 248 | * create meta package and edit LICENSE and README 249 | * Contributors: Darby Lim, pyo 250 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_tool.cpp: -------------------------------------------------------------------------------- 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), Ryan Shim */ 18 | 19 | #include "../../include/dynamixel_workbench_toolbox/dynamixel_tool.h" 20 | 21 | //=================================================================== 22 | // Define Serial ID to Namd table 23 | //=================================================================== 24 | typedef struct 25 | { 26 | uint16_t number; 27 | const char* name; 28 | } DynamixelModel; 29 | 30 | static const DynamixelModel dynamixel_model_table[] = { 31 | {AX_12A, "AX-12A"}, 32 | {AX_12W, "AX-12W"}, 33 | {AX_18A, "AX-18A"}, 34 | 35 | {RX_10, "RX-10"}, 36 | {RX_24F, "RX-24F"}, 37 | {RX_28, "RX-28"}, 38 | {RX_64, "RX-64"}, 39 | 40 | {EX_106, "EX-106"}, 41 | 42 | {MX_12W, "MX-12W"}, 43 | {MX_28, "MX-28"}, 44 | {MX_28_2, "MX-28-2"}, 45 | {MX_64, "MX-64"}, 46 | {MX_64_2, "MX-64-2"}, 47 | {MX_106, "MX-106"}, 48 | {MX_106_2, "MX-106-2"}, 49 | 50 | {XL_320, "XL-320"}, 51 | {XL330_M077, "XL330_M077"}, 52 | {XL330_M288, "XL330_M288"}, 53 | {XC330_M181, "XC330_M181"}, 54 | {XC330_M288, "XC330_M288"}, 55 | {XC330_T181, "XC330_T181"}, 56 | {XC330_T288, "XC330_T288"}, 57 | 58 | {XL430_W250, "XL430-W250"}, 59 | {XL430_W250_2, "XL430-W250-2"}, // 2XL 60 | {XC430_W250_2, "XC430-W250-2"}, // 2XC 61 | 62 | {XC430_W150, "XC430-W150"}, 63 | {XC430_W240, "XC430-W240"}, 64 | 65 | {XM430_W210, "XM430-W210"}, 66 | {XM430_W350, "XM430-W350"}, 67 | 68 | {XM540_W150, "XM540-W150"}, 69 | {XM540_W270, "XM540-W270"}, 70 | 71 | {XH430_V210, "XH430-V210"}, 72 | {XH430_V350, "XH430-V350"}, 73 | {XH430_W210, "XH430-W210"}, 74 | {XH430_W350, "XH430-W350"}, 75 | 76 | {XH540_W150, "XH540_W150"}, 77 | {XH540_W270, "XH540_W270"}, 78 | {XH540_V150, "XH540_V150"}, 79 | {XH540_V270, "XH540_V270"}, 80 | 81 | {XW430_T200, "XW430-T200"}, 82 | {XW430_T333, "XW430-T333"}, 83 | {XW540_T260, "XW540_T260"}, 84 | {XW540_T140, "XW540_T140"}, 85 | 86 | {PRO_L42_10_S300_R, "PRO-L42-10-S300-R"}, 87 | {PRO_L54_30_S400_R, "PRO-L54-30-S400-R"}, 88 | {PRO_L54_30_S500_R, "PRO-L54-30-S500-R"}, 89 | {PRO_L54_50_S290_R, "PRO-L54-50-S290-R"}, 90 | {PRO_L54_50_S500_R, "PRO-L54-50-S500-R"}, 91 | 92 | {PRO_M42_10_S260_R, "PRO-M42-10-S260-R"}, 93 | {PRO_M54_40_S250_R, "PRO-M54-40-S250-R"}, 94 | {PRO_M54_60_S250_R, "PRO-M54-60-S250-R"}, 95 | 96 | {PRO_H42_20_S300_R, "PRO-H42-20-S300-R"}, 97 | {PRO_H54_100_S500_R, "PRO-H54-100-S500-R"}, 98 | {PRO_H54_200_S500_R, "PRO-H54-200-S500-R"}, 99 | 100 | {PRO_M42_10_S260_R_A, "PRO-M42-10-S260-R-A"}, 101 | {PRO_M54_40_S250_R_A, "PRO-M54-40-S250-R-A"}, 102 | {PRO_M54_60_S250_R_A, "PRO-M54-60-S250-R-A"}, 103 | 104 | {PRO_H42_20_S300_R_A, "PRO-H42-20-S300-R-A"}, 105 | {PRO_H54_100_S500_R_A, "PRO-H54-100-S500-R-A"}, 106 | {PRO_H54_200_S500_R_A, "PRO-H54-200-S500-R-A"}, 107 | 108 | {PRO_PLUS_M42P_010_S260_R, "PRO-PLUS-M42P-010-S260-R"}, 109 | {PRO_PLUS_M54P_040_S250_R, "PRO-PLUS-M54P-040-S250-R"}, 110 | {PRO_PLUS_M54P_060_S250_R, "PRO-PLUS-M54P-060-S250-R"}, 111 | 112 | {PRO_PLUS_H42P_020_S300_R, "PRO-PLUS-H42P-020-S300-R"}, 113 | {PRO_PLUS_H54P_100_S500_R, "PRO-PLUS-H54P-100-S500-R"}, 114 | {PRO_PLUS_H54P_200_S500_R, "PRO-PLUS-H54P-200-S500-R"}, 115 | 116 | {RH_P12_RN, "RH-P12-RN"}, 117 | 118 | {RH_P12_RN_A, "RH-P12-RN-A"} 119 | }; 120 | #define COUNT_DYNAMIXEL_MODEL (sizeof(dynamixel_model_table)/sizeof(dynamixel_model_table[0])) 121 | 122 | DynamixelTool::DynamixelTool() : dxl_cnt_(0), the_number_of_control_item_(0){} 123 | 124 | DynamixelTool::~DynamixelTool(){} 125 | 126 | void DynamixelTool::initTool(void) 127 | { 128 | for (uint8_t i = 0; i < DYNAMIXEL_BUFFER; i++) 129 | dxl_id_[i] = 0; 130 | 131 | dxl_cnt_ = 0; 132 | } 133 | 134 | bool DynamixelTool::addTool(const char *model_name, uint8_t id, const char **log) 135 | { 136 | bool result = false; 137 | initTool(); 138 | 139 | model_name_ = model_name; 140 | result = setModelNumber(model_name, log); 141 | if (result == false) return false; 142 | dxl_id_[dxl_cnt_++] = id; 143 | 144 | result = setControlTable(model_name, log); 145 | if (result == false) return false; 146 | 147 | return true; 148 | } 149 | 150 | bool DynamixelTool::addTool(uint16_t model_number, uint8_t id, const char **log) 151 | { 152 | bool result = false; 153 | initTool(); 154 | 155 | result = setModelName(model_number, log); 156 | if (result == false) return false; 157 | model_number_ = model_number; 158 | dxl_id_[dxl_cnt_++] = id; 159 | 160 | result = setControlTable(model_number, log); 161 | if (result == false) return false; 162 | 163 | return result; 164 | } 165 | 166 | void DynamixelTool::addDXL(uint8_t id) 167 | { 168 | dxl_id_[dxl_cnt_++] = id; 169 | } 170 | 171 | bool DynamixelTool::setControlTable(const char *model_name, const char **log) 172 | { 173 | const char* name = model_name; 174 | uint8_t name_length = strlen(name); 175 | 176 | for (uint8_t index=0; index < COUNT_DYNAMIXEL_MODEL; index++) 177 | { 178 | if(strncmp(name, dynamixel_model_table[index].name, name_length) == 0) 179 | { 180 | return setControlTable(dynamixel_model_table[index].number, log); 181 | } 182 | } 183 | 184 | if (log != NULL) 185 | *log = "[DynamixelTool] Failed to set control table due to mismatch model name and model number"; 186 | return false; 187 | } 188 | 189 | bool DynamixelTool::setControlTable(uint16_t model_number, const char **log) 190 | { 191 | control_table_ = DynamixelItem::getControlTable(model_number); 192 | the_number_of_control_item_ = DynamixelItem::getTheNumberOfControlItem(); 193 | model_info_ = DynamixelItem::getModelInfo(model_number); 194 | 195 | if (control_table_ == NULL || model_info_ == NULL) 196 | { 197 | if (log != NULL) 198 | *log = "[DynamixelTool] Failed to get control table or model info"; 199 | return false; 200 | } 201 | 202 | return true; 203 | } 204 | 205 | bool DynamixelTool::setModelName(uint16_t model_number, const char **log) 206 | { 207 | uint16_t num = model_number; 208 | 209 | for (uint8_t index=0; index < COUNT_DYNAMIXEL_MODEL; index++) 210 | { 211 | if (num == dynamixel_model_table[index].number) 212 | { 213 | model_name_ = dynamixel_model_table[index].name; 214 | return true; 215 | } 216 | } 217 | 218 | if (log != NULL) 219 | *log = "[DynamixelTool] Failed to find model name"; 220 | return false; 221 | } 222 | 223 | bool DynamixelTool::setModelNumber(const char *model_name, const char **log) 224 | { 225 | const char* name = model_name; 226 | uint8_t name_length = strlen(name); 227 | 228 | for (uint8_t index=0; index < COUNT_DYNAMIXEL_MODEL; index++) 229 | { 230 | if(strncmp(name, model_name_, name_length) == 0) 231 | { 232 | model_number_ = dynamixel_model_table[index].number; 233 | return true; 234 | } 235 | } 236 | 237 | if (log != NULL) 238 | *log = "[DynamixelTool] Failed to find model number"; 239 | return false; 240 | } 241 | 242 | const char* DynamixelTool::getModelName(void) 243 | { 244 | return model_name_; 245 | } 246 | 247 | uint16_t DynamixelTool::getModelNumber(void) 248 | { 249 | return model_number_; 250 | } 251 | 252 | const uint8_t* DynamixelTool::getID(void) 253 | { 254 | const uint8_t* id_table_ = dxl_id_; 255 | 256 | return id_table_; 257 | } 258 | 259 | uint8_t DynamixelTool::getDynamixelCount(void) 260 | { 261 | return dxl_cnt_; 262 | } 263 | 264 | uint8_t DynamixelTool::getDynamixelBuffer(void) 265 | { 266 | return DYNAMIXEL_BUFFER; 267 | } 268 | 269 | float DynamixelTool::getRPM(void) 270 | { 271 | return model_info_->rpm; 272 | } 273 | 274 | int64_t DynamixelTool::getValueOfMinRadianPosition(void) 275 | { 276 | return model_info_->value_of_min_radian_position; 277 | } 278 | 279 | int64_t DynamixelTool::getValueOfMaxRadianPosition(void) 280 | { 281 | return model_info_->value_of_max_radian_position; 282 | } 283 | 284 | int64_t DynamixelTool::getValueOfZeroRadianPosition(void) 285 | { 286 | return model_info_->value_of_zero_radian_position; 287 | } 288 | 289 | float DynamixelTool::getMinRadian(void) 290 | { 291 | return model_info_->min_radian; 292 | } 293 | 294 | float DynamixelTool::getMaxRadian(void) 295 | { 296 | return model_info_->max_radian; 297 | } 298 | 299 | uint8_t DynamixelTool::getTheNumberOfControlItem(void) 300 | { 301 | return the_number_of_control_item_; 302 | } 303 | 304 | const ControlItem *DynamixelTool::getControlItem(const char *item_name, const char **log) 305 | { 306 | const ControlItem* control_item = control_table_; 307 | uint8_t name_length = strlen(item_name); 308 | 309 | for (uint8_t num = 0; num < the_number_of_control_item_; num++) 310 | { 311 | if ((name_length == control_item->item_name_length) && 312 | (memcmp(item_name, control_item->item_name, name_length) == 0)) 313 | { 314 | return control_item; 315 | } 316 | control_item++; 317 | } 318 | 319 | if (log != NULL) 320 | *log = "[DynamixelTool] Can't find Item"; 321 | return NULL; 322 | } 323 | 324 | const ControlItem *DynamixelTool::getControlTable(void) 325 | { 326 | return control_table_; 327 | } 328 | 329 | const ModelInfo *DynamixelTool::getModelInfo(void) 330 | { 331 | return model_info_; 332 | } 333 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/examples/src/p_Monitor.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | Copyright 2016 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 20 | #include // FILE control 21 | #include // Terminal IO 22 | #include 23 | 24 | using namespace std; 25 | 26 | #define ESC_ASCII_VALUE 0x1b 27 | #define SPACEBAR_ASCII_VALUE 0x20 28 | #define ENTER_ASCII_VALUE 0x0a 29 | 30 | uint8_t get_id[16]; 31 | uint8_t scan_cnt = 0; 32 | uint8_t ping_cnt = 0; 33 | 34 | int getch(void); 35 | int kbhit(void); 36 | bool isAvailableID(uint8_t id); 37 | void printInst(); 38 | bool monitoring(const char* port_name); 39 | 40 | DynamixelWorkbench dxl_wb; 41 | 42 | int main(int argc, char *argv[]) 43 | { 44 | const char* port_name = "/dev/ttyUSB0"; 45 | 46 | if (argc < 2) 47 | { 48 | printf("Please set '-port_name' arguments for connected Dynamixels\n"); 49 | return 0; 50 | } 51 | else 52 | { 53 | port_name = argv[1]; 54 | } 55 | 56 | printInst(); 57 | 58 | while(true) 59 | { 60 | monitoring(port_name); 61 | } 62 | 63 | return 0; 64 | } 65 | 66 | bool monitoring(const char* port_name) 67 | { 68 | char input[128]; 69 | char cmd[80]; 70 | char param[20][30]; 71 | int num_param = 0; 72 | char *token; 73 | bool valid_cmd = false; 74 | 75 | bool wb_result = false; 76 | const char *log; 77 | 78 | if (kbhit()) 79 | { 80 | if (getchar() == ENTER_ASCII_VALUE) 81 | { 82 | printf("[CMD]"); 83 | fgets(input, sizeof(input), stdin); 84 | 85 | char *p; 86 | if ((p = strchr(input, '\n'))!= NULL) *p = '\0'; 87 | fflush(stdin); 88 | 89 | if (strlen(input) == 0) return false; 90 | 91 | token = strtok(input, " "); 92 | 93 | if (token == 0) return false; 94 | 95 | strcpy(cmd, token); 96 | token = strtok(0, " "); 97 | num_param = 0; 98 | 99 | while (token != 0) 100 | { 101 | strcpy(param[num_param++], token); 102 | token = strtok(0, " "); 103 | } 104 | 105 | if (strcmp(cmd, "help") == 0 || strcmp(cmd, "h") == 0 || strcmp(cmd, "?") == 0) 106 | { 107 | printInst(); 108 | } 109 | else if (strcmp(cmd, "begin") == 0) 110 | { 111 | uint32_t baud = 57600; 112 | 113 | baud = atoi(param[0]); 114 | wb_result = dxl_wb.init(port_name, baud, &log); 115 | if (wb_result == false) 116 | { 117 | printf("%s\n", log); 118 | printf("Failed to init\n"); 119 | } 120 | else 121 | printf("Succeed to init(%d)\n", baud); 122 | } 123 | else if (strcmp(cmd, "end") == 0) 124 | { 125 | exit(0); 126 | } 127 | else if (strcmp(cmd, "scan") == 0) 128 | { 129 | uint8_t range = 253; // default 130 | 131 | range = atoi(param[0]); 132 | wb_result = dxl_wb.scan(get_id, &scan_cnt, range, &log); 133 | if (wb_result == false) 134 | { 135 | printf("%s\n", log); 136 | printf("Failed to scan\n"); 137 | } 138 | else 139 | { 140 | printf("Find %d Dynamixels\n", scan_cnt); 141 | 142 | for (int cnt = 0; cnt < scan_cnt; cnt++) 143 | printf("id : %d\n", get_id[cnt]); 144 | } 145 | } 146 | else if (strcmp(cmd, "ping") == 0) 147 | { 148 | get_id[ping_cnt] = 1; // default to 1 149 | 150 | get_id[ping_cnt] = atoi(param[0]); 151 | 152 | uint16_t model_number = 0; 153 | 154 | wb_result = dxl_wb.ping(get_id[ping_cnt], &model_number, &log); 155 | if (wb_result == false) 156 | { 157 | printf("%s\n", log); 158 | printf("Failed to ping\n"); 159 | } 160 | else 161 | { 162 | printf("id : %d, model_number : %d\n", get_id[ping_cnt], model_number); 163 | ping_cnt++; 164 | } 165 | } 166 | else if (isAvailableID(atoi(param[0]))) 167 | { 168 | if (strcmp(cmd, "control_table") == 0) 169 | { 170 | uint8_t id = atoi(param[0]); 171 | 172 | const ControlItem *control_item = dxl_wb.getControlTable(id); 173 | uint8_t the_number_of_control_item = dxl_wb.getTheNumberOfControlItem(id); 174 | 175 | uint16_t last_register_addr = control_item[the_number_of_control_item-1].address; 176 | uint16_t last_register_addr_length = control_item[the_number_of_control_item-1].data_length; 177 | 178 | uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length]; 179 | 180 | if (control_item != NULL) 181 | { 182 | wb_result = dxl_wb.readRegister(id, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData, &log); 183 | if (wb_result == false) 184 | { 185 | printf("%s\n", log); 186 | return 0; 187 | } 188 | else 189 | { 190 | for (int index = 0; index < the_number_of_control_item; index++) 191 | { 192 | uint32_t data = 0; 193 | 194 | if (dxl_wb.getProtocolVersion() == 2.0f) 195 | { 196 | data = getAllRegisteredData[control_item[index].address]; 197 | printf("\t%s : %d\n", control_item[index].item_name, data); 198 | } 199 | else if (dxl_wb.getProtocolVersion() == 1.0f) 200 | { 201 | switch (control_item[index].data_length) 202 | { 203 | case BYTE: 204 | data = getAllRegisteredData[control_item[index].address]; 205 | printf("\t%s : %d\n", control_item[index].item_name, data); 206 | break; 207 | 208 | case WORD: 209 | data = DXL_MAKEWORD(getAllRegisteredData[control_item[index].address], getAllRegisteredData[control_item[index].address+1]); 210 | printf("\t%s : %d\n", control_item[index].item_name, data); 211 | break; 212 | 213 | case DWORD: 214 | data = DXL_MAKEDWORD(DXL_MAKEWORD(getAllRegisteredData[control_item[index].address], getAllRegisteredData[control_item[index].address+1]), 215 | DXL_MAKEWORD(getAllRegisteredData[control_item[index].address+2], getAllRegisteredData[control_item[index].address+3])); 216 | printf("\t%s : %d\n", control_item[index].item_name, data); 217 | break; 218 | 219 | default: 220 | data = getAllRegisteredData[control_item[index].address]; 221 | break; 222 | } 223 | } 224 | } 225 | } 226 | } 227 | } 228 | else if (strcmp(cmd, "sync_write_handler") == 0) 229 | { 230 | static uint8_t sync_write_handler_index = 0; 231 | uint8_t id = atoi(param[0]); 232 | wb_result = dxl_wb.addSyncWriteHandler(id, param[1], &log); 233 | if (wb_result == false) 234 | { 235 | printf("%s\n", log); 236 | printf("Failed to add sync write handler\n"); 237 | return 0; 238 | } 239 | else 240 | printf("%s, sync_write_handler_index = %d\n", log, sync_write_handler_index++); 241 | } 242 | else if (strcmp(cmd, "sync_read_handler") == 0) 243 | { 244 | static uint8_t sync_read_handler_index = 0; 245 | uint8_t id = atoi(param[0]); 246 | wb_result = dxl_wb.addSyncReadHandler(id, param[1], &log); 247 | if (wb_result == false) 248 | { 249 | printf("%s\n", log); 250 | printf("Failed to add sync read handler\n"); 251 | return 0; 252 | } 253 | else 254 | printf("%s, sync_read_handler_index = %d\n", log, sync_read_handler_index++); 255 | } 256 | else if (strcmp(cmd, "bulk_write_handler") == 0) 257 | { 258 | wb_result = dxl_wb.initBulkWrite(&log); 259 | if (wb_result == false) 260 | { 261 | printf("%s\n", log); 262 | printf("Failed to init bulk write handler\n"); 263 | return 0; 264 | } 265 | else 266 | printf("%s\n", log); 267 | } 268 | else if (strcmp(cmd, "bulk_write_param") == 0) 269 | { 270 | uint8_t id = atoi(param[0]); 271 | wb_result = dxl_wb.addBulkWriteParam(id, param[1], atoi(param[2]), &log); 272 | if (wb_result == false) 273 | { 274 | printf("%s\n", log); 275 | printf("Failed to add param for bulk write\n"); 276 | return 0; 277 | } 278 | else 279 | printf("%s\n", log); 280 | } 281 | else if (strcmp(cmd, "bulk_write") == 0) 282 | { 283 | wb_result = dxl_wb.bulkWrite(&log); 284 | if (wb_result == false) 285 | { 286 | printf("%s\n", log); 287 | printf("Failed to bulk write\n"); 288 | return 0; 289 | } 290 | else 291 | printf("%s\n", log); 292 | } 293 | else if (strcmp(cmd, "bulk_read_handler") == 0) 294 | { 295 | wb_result = dxl_wb.initBulkRead(&log); 296 | if (wb_result == false) 297 | { 298 | printf("%s\n", log); 299 | printf("Failed to init bulk read handler\n"); 300 | return 0; 301 | } 302 | else 303 | printf("%s\n", log); 304 | } 305 | else if (strcmp(cmd, "bulk_read_param") == 0) 306 | { 307 | uint8_t id = atoi(param[0]); 308 | wb_result = dxl_wb.addBulkReadParam(id, param[1], &log); 309 | if (wb_result == false) 310 | { 311 | printf("%s\n", log); 312 | printf("Failed to add param for bulk read\n"); 313 | return 0; 314 | } 315 | else 316 | printf("%s\n", log); 317 | } 318 | else if (strcmp(cmd, "bulk_read") == 0) 319 | { 320 | wb_result = dxl_wb.bulkRead(&log); 321 | if (wb_result == false) 322 | { 323 | printf("%s\n", log); 324 | printf("Failed to bulk read\n"); 325 | return 0; 326 | } 327 | else 328 | printf("%s\n", log); 329 | 330 | int32_t get_data[dxl_wb.getTheNumberOfBulkReadParam()]; 331 | wb_result = dxl_wb.getBulkReadData(&get_data[0], &log); 332 | if (wb_result == false) 333 | { 334 | printf("%s\n", log); 335 | printf("Failed to get bulk read data\n"); 336 | return 0; 337 | } 338 | else 339 | { 340 | printf("%s\n", log); 341 | for (uint8_t index = 0; index < dxl_wb.getTheNumberOfBulkReadParam(); index++) 342 | printf("data[%d] : %d ", index, get_data[index]); 343 | 344 | printf("\n"); 345 | } 346 | 347 | dxl_wb.clearBulkReadParam(); 348 | } 349 | else if (isAvailableID(atoi(param[0])) && isAvailableID(atoi(param[1]))) 350 | { 351 | if (strcmp(cmd, "sync_write") == 0) 352 | { 353 | uint8_t id_1 = atoi(param[0]); 354 | uint8_t id_2 = atoi(param[1]); 355 | uint8_t id[2] = {id_1, id_2}; 356 | uint8_t id_num = 2; 357 | 358 | int32_t data[2] = {0, 0}; 359 | data[0] = atoi(param[3]); 360 | data[1] = atoi(param[4]); 361 | 362 | uint8_t handler_index = atoi(param[2]); 363 | 364 | wb_result = dxl_wb.syncWrite(handler_index, id, id_num, (int32_t *)data, 1, &log); 365 | if (wb_result == false) 366 | { 367 | printf("%s\n", log); 368 | return 0; 369 | } 370 | else 371 | printf("%s\n", log); 372 | } 373 | else if (strcmp(cmd, "sync_read") == 0) 374 | { 375 | uint8_t id_1 = atoi(param[0]); 376 | uint8_t id_2 = atoi(param[1]); 377 | uint8_t id[2] = {id_1, id_2}; 378 | uint8_t id_num = 2; 379 | 380 | int32_t data[2] = {0, 0}; 381 | uint8_t handler_index = atoi(param[2]); 382 | 383 | wb_result = dxl_wb.syncRead(handler_index, id, id_num, &log); 384 | if (wb_result == false) 385 | { 386 | printf("%s\n", log); 387 | return 0; 388 | } 389 | else 390 | { 391 | printf("%s\n", log); 392 | } 393 | 394 | wb_result = dxl_wb.getSyncReadData(handler_index, id, id_num, data, &log); 395 | if (wb_result == false) 396 | { 397 | printf("%s\n", log); 398 | return 0; 399 | } 400 | else 401 | { 402 | printf("%s\n", log); 403 | printf("id[%d] : %d, id[%d] : %d\n", id_1, data[0], id_2, data[1]); 404 | } 405 | } 406 | } 407 | else if (strcmp(cmd, "id") == 0) 408 | { 409 | uint8_t id = atoi(param[0]); 410 | uint8_t new_id = atoi(param[1]); 411 | 412 | wb_result = dxl_wb.changeID(id, new_id, &log); 413 | if (wb_result == false) 414 | { 415 | printf("%s\n", log); 416 | return 0; 417 | } 418 | else 419 | { 420 | printf("%s\n", log); 421 | } 422 | } 423 | else if (strcmp(cmd, "baud") == 0) 424 | { 425 | uint8_t id = atoi(param[0]); 426 | uint32_t new_baud = atoi(param[1]); 427 | 428 | wb_result = dxl_wb.changeBaudrate(id, new_baud, &log); 429 | if (wb_result == false) 430 | { 431 | printf("%s\n", log); 432 | return 0; 433 | } 434 | else 435 | { 436 | wb_result = dxl_wb.setBaudrate(new_baud, &log); 437 | printf("%s\n", log); 438 | } 439 | } 440 | else if (strcmp(cmd, "torque_on") == 0) 441 | { 442 | uint8_t id = atoi(param[0]); 443 | 444 | wb_result = dxl_wb.torqueOn(id, &log); 445 | if (wb_result == false) 446 | { 447 | printf("%s\n", log); 448 | return 0; 449 | } 450 | else 451 | { 452 | printf("%s\n", log); 453 | } 454 | } 455 | else if (strcmp(cmd, "torque_off") == 0) 456 | { 457 | uint8_t id = atoi(param[0]); 458 | 459 | wb_result = dxl_wb.torqueOff(id, &log); 460 | if (wb_result == false) 461 | { 462 | printf("%s\n", log); 463 | return 0; 464 | } 465 | else 466 | { 467 | printf("%s\n", log); 468 | } 469 | } 470 | else if (strcmp(cmd, "joint") == 0) 471 | { 472 | uint8_t id = atoi(param[0]); 473 | uint32_t goal = atoi(param[1]); 474 | 475 | wb_result = dxl_wb.jointMode(id, 0, 0, &log); 476 | if (wb_result == false) 477 | { 478 | printf("%s\n", log); 479 | return 0; 480 | } 481 | else 482 | { 483 | printf("%s\n", log); 484 | } 485 | 486 | wb_result = dxl_wb.goalPosition(id, (int32_t)goal, &log); 487 | if (wb_result == false) 488 | { 489 | printf("%s\n", log); 490 | return 0; 491 | } 492 | else 493 | { 494 | printf("%s\n", log); 495 | } 496 | } 497 | else if (strcmp(cmd, "wheel") == 0) 498 | { 499 | uint8_t id = atoi(param[0]); 500 | uint32_t goal = atoi(param[1]); 501 | 502 | wb_result = dxl_wb.wheelMode(id, 0, &log); 503 | if (wb_result == false) 504 | { 505 | printf("%s\n", log); 506 | return 0; 507 | } 508 | else 509 | { 510 | printf("%s\n", log); 511 | } 512 | 513 | wb_result = dxl_wb.goalVelocity(id, (int32_t)goal, &log); 514 | if (wb_result == false) 515 | { 516 | printf("%s\n", log); 517 | return 0; 518 | } 519 | else 520 | { 521 | printf("%s\n", log); 522 | } 523 | } 524 | else if (strcmp(cmd, "write") == 0) 525 | { 526 | uint8_t id = atoi(param[0]); 527 | int32_t value = atoi(param[2]); 528 | 529 | wb_result = dxl_wb.writeRegister(id, param[1], value, &log); 530 | if (wb_result == false) 531 | { 532 | printf("%s\n", log); 533 | printf("Failed to write\n"); 534 | return 0; 535 | } 536 | else 537 | { 538 | printf("%s\n", log); 539 | } 540 | } 541 | else if (strcmp(cmd, "read") == 0) 542 | { 543 | uint8_t id = atoi(param[0]); 544 | 545 | int32_t data = 0; 546 | 547 | wb_result = dxl_wb.readRegister(id, param[1], &data, &log); 548 | if (wb_result == false) 549 | { 550 | printf("%s\n", log); 551 | printf("Failed to read\n"); 552 | return 0; 553 | } 554 | else 555 | { 556 | printf("%s\n", log); 557 | printf("read data : %d\n", data); 558 | } 559 | } 560 | else if (strcmp(cmd, "reboot") == 0) 561 | { 562 | uint8_t id = atoi(param[0]); 563 | 564 | wb_result = dxl_wb.reboot(id, &log); 565 | if (wb_result == false) 566 | { 567 | printf("%s\n", log); 568 | printf("Failed to reboot\n"); 569 | return 0; 570 | } 571 | else 572 | { 573 | printf("%s\n", log); 574 | } 575 | } 576 | else if (strcmp(cmd, "reset") == 0) 577 | { 578 | uint8_t id = atoi(param[0]); 579 | 580 | wb_result = dxl_wb.reset(id, &log); 581 | if (wb_result == false) 582 | { 583 | printf("%s\n", log); 584 | printf("Failed to reset\n"); 585 | return 0; 586 | } 587 | else 588 | { 589 | printf("%s\n", log); 590 | } 591 | } 592 | else 593 | { 594 | printf("Wrong command\n"); 595 | } 596 | } 597 | else 598 | { 599 | printf("Please check ID\n"); 600 | } 601 | } 602 | } 603 | 604 | return 0; 605 | } 606 | 607 | bool isAvailableID(uint8_t id) 608 | { 609 | for (int dxl_cnt = 0; dxl_cnt < (scan_cnt + ping_cnt); dxl_cnt++) 610 | { 611 | if (get_id[dxl_cnt] == id) 612 | return true; 613 | } 614 | 615 | return false; 616 | } 617 | 618 | void printInst(void) 619 | { 620 | printf("-------------------------------------\n"); 621 | printf("Set begin before scan or ping\n"); 622 | printf("-------------------------------------\n"); 623 | printf("help\n"); 624 | printf("begin (BAUD)\n"); 625 | printf("scan (RANGE)\n"); 626 | printf("ping (ID)\n"); 627 | printf("control_table (ID)\n"); 628 | printf("id (ID) (NEW_ID)\n"); 629 | printf("baud (ID) (NEW_BAUD)\n"); 630 | printf("torque_on (ID)\n"); 631 | printf("torque_off (ID)\n"); 632 | printf("joint (ID) (GOAL_POSITION)\n"); 633 | printf("wheel (ID) (GOAL_VELOCITY)\n"); 634 | printf("write (ID) (ADDRESS_NAME) (DATA)\n"); 635 | printf("read (ID) (ADDRESS_NAME)\n"); 636 | printf("sync_write_handler (Ref_ID) (ADDRESS_NAME)\n"); 637 | printf("sync_write (ID_1) (ID_2) (HANDLER_INDEX) (PARAM_1) (PARAM_2)\n"); 638 | printf("sync_read_handler (Ref_ID) (ADDRESS_NAME)\n"); 639 | printf("sync_read (ID_1) (ID_2) (HANDLER_INDEX)\n"); 640 | printf("bulk_write_handler\n"); 641 | printf("bulk_write_param (ID) (ADDRESS_NAME) (PARAM)\n"); 642 | printf("bulk_write\n"); 643 | printf("bulk_read_handler\n"); 644 | printf("bulk_read_param (ID) (ADDRESS_NAME)\n"); 645 | printf("bulk_read\n"); 646 | printf("reboot (ID) \n"); 647 | printf("reset (ID) \n"); 648 | printf("end\n"); 649 | printf("-------------------------------------\n"); 650 | printf("Press Enter Key\n"); 651 | } 652 | 653 | int getch() 654 | { 655 | struct termios oldt, newt; 656 | int ch; 657 | 658 | tcgetattr( STDIN_FILENO, &oldt ); 659 | newt = oldt; 660 | newt.c_lflag &= ~(ICANON | ECHO); 661 | newt.c_cc[VMIN] = 0; 662 | newt.c_cc[VTIME] = 1; 663 | tcsetattr( STDIN_FILENO, TCSANOW, &newt ); 664 | ch = getchar(); 665 | tcsetattr( STDIN_FILENO, TCSANOW, &oldt ); 666 | 667 | return ch; 668 | } 669 | 670 | int kbhit() 671 | { 672 | struct termios oldt, newt; 673 | int ch; 674 | int oldf; 675 | 676 | tcgetattr(STDIN_FILENO, &oldt); 677 | newt = oldt; 678 | newt.c_lflag &= ~(ICANON | ECHO); 679 | tcsetattr(STDIN_FILENO, TCSANOW, &newt); 680 | oldf = fcntl(STDIN_FILENO, F_GETFL, 0); 681 | fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); 682 | 683 | ch = getchar(); 684 | 685 | tcsetattr(STDIN_FILENO, TCSANOW, &oldt); 686 | fcntl(STDIN_FILENO, F_SETFL, oldf); 687 | 688 | if (ch != EOF) 689 | { 690 | ungetc(ch, stdin); 691 | return 1; 692 | } 693 | return 0; 694 | } 695 | -------------------------------------------------------------------------------- /dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp: -------------------------------------------------------------------------------- 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), Ryan Shim */ 18 | 19 | #include "../../include/dynamixel_workbench_toolbox/dynamixel_workbench.h" 20 | 21 | static const uint8_t WHEEL_MODE = 1; 22 | static const uint8_t JOINT_MODE = 2; 23 | 24 | static const uint8_t CURRENT_CONTROL_MODE = 0; 25 | static const uint8_t VELOCITY_CONTROL_MODE = 1; 26 | static const uint8_t POSITION_CONTROL_MODE = 3; 27 | static const uint8_t EXTENDED_POSITION_CONTROL_MODE = 4; 28 | static const uint8_t CURRENT_BASED_POSITION_CONTROL_MODE = 5; 29 | static const uint8_t PWM_CONTROL_MODE = 16; 30 | static const uint8_t TORQUE_CONTROL_MODE = 100; 31 | static const uint8_t MULTI_TURN_MODE = 101; 32 | 33 | static const char* model_name = NULL; 34 | static const ModelInfo* model_info = NULL; 35 | 36 | DynamixelWorkbench::DynamixelWorkbench(){} 37 | 38 | DynamixelWorkbench::~DynamixelWorkbench(){} 39 | 40 | bool DynamixelWorkbench::torque(uint8_t id, int32_t onoff, const char **log) 41 | { 42 | bool result = false; 43 | 44 | result = itemWrite(id, "Torque_Enable", (int32_t)onoff, log); 45 | if (result == false) 46 | { 47 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to change torque status!"; 48 | return false; 49 | } 50 | 51 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change torque status!"; 52 | return result; 53 | } 54 | 55 | bool DynamixelWorkbench::torqueOn(uint8_t id, const char **log) 56 | { 57 | bool result = false; 58 | 59 | result = torque(id, 1, log); 60 | 61 | return result; 62 | } 63 | 64 | bool DynamixelWorkbench::torqueOff(uint8_t id, const char **log) 65 | { 66 | bool result = false; 67 | 68 | result = torque(id, 0, log); 69 | 70 | return result; 71 | } 72 | 73 | bool DynamixelWorkbench::changeID(uint8_t id, uint8_t new_id, const char **log) 74 | { 75 | bool result = false; 76 | 77 | result = torqueOff(id, log); 78 | if (result == false) return false; 79 | 80 | result = writeRegister(id, "ID", new_id, log); 81 | if (result == false) 82 | { 83 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to change ID!"; 84 | return false; 85 | } 86 | // millis(1000); 87 | 88 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change ID!"; 89 | return result; 90 | } 91 | 92 | bool DynamixelWorkbench::changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log) 93 | { 94 | bool result = false; 95 | 96 | result = torqueOff(id, log); 97 | if (result == false) return false; 98 | 99 | if (getProtocolVersion() == 1.0f) 100 | { 101 | switch (new_baudrate) 102 | { 103 | case 9600: 104 | result = writeRegister(id, "Baud_Rate", 207, log); 105 | break; 106 | 107 | case 19200: 108 | result = writeRegister(id, "Baud_Rate", 103, log); 109 | break; 110 | 111 | case 57600: 112 | result = writeRegister(id, "Baud_Rate", 34, log); 113 | break; 114 | 115 | case 115200: 116 | result = writeRegister(id, "Baud_Rate", 16, log); 117 | break; 118 | 119 | case 200000: 120 | result = writeRegister(id, "Baud_Rate", 9, log); 121 | break; 122 | 123 | case 250000: 124 | result = writeRegister(id, "Baud_Rate", 7, log); 125 | break; 126 | 127 | case 400000: 128 | result = writeRegister(id, "Baud_Rate", 4, log); 129 | break; 130 | 131 | case 500000: 132 | result = writeRegister(id, "Baud_Rate", 3, log); 133 | break; 134 | 135 | case 1000000: 136 | result = writeRegister(id, "Baud_Rate", 1, log); 137 | break; 138 | 139 | case 2250000: 140 | result = writeRegister(id, "Baud_Rate", 250, log); 141 | break; 142 | 143 | case 2500000: 144 | result = writeRegister(id, "Baud_Rate", 251, log); 145 | break; 146 | 147 | case 3000000: 148 | result = writeRegister(id, "Baud_Rate", 252, log); 149 | break; 150 | 151 | default: 152 | result = writeRegister(id, "Baud_Rate", 34, log); 153 | break; 154 | } 155 | } 156 | else if (getProtocolVersion() == 2.0f) 157 | { 158 | switch (new_baudrate) 159 | { 160 | case 9600: 161 | result = writeRegister(id, "Baud_Rate", 0, log); 162 | break; 163 | 164 | case 57600: 165 | result = writeRegister(id, "Baud_Rate", 1, log); 166 | break; 167 | 168 | case 115200: 169 | result = writeRegister(id, "Baud_Rate", 2, log); 170 | break; 171 | 172 | case 1000000: 173 | result = writeRegister(id, "Baud_Rate", 3, log); 174 | break; 175 | 176 | case 2000000: 177 | result = writeRegister(id, "Baud_Rate", 4, log); 178 | break; 179 | 180 | case 3000000: 181 | result = writeRegister(id, "Baud_Rate", 5, log); 182 | break; 183 | 184 | case 4000000: 185 | result = writeRegister(id, "Baud_Rate", 6, log); 186 | break; 187 | 188 | case 4500000: 189 | result = writeRegister(id, "Baud_Rate", 7, log); 190 | break; 191 | 192 | case 10500000: 193 | result = writeRegister(id, "Baud_Rate", 8, log); 194 | break; 195 | 196 | default: 197 | result = writeRegister(id, "Baud_Rate", 1, log); 198 | break; 199 | } 200 | } 201 | #if defined(__OPENCR__) || defined(__OPENCM904__) 202 | delay(2000); 203 | #else 204 | usleep(1000*2000); 205 | #endif 206 | 207 | if (result == false) 208 | { 209 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to change Baud Rate!"; 210 | return result; 211 | } 212 | 213 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change Baud Rate!"; 214 | return result; 215 | } 216 | 217 | bool DynamixelWorkbench::changeProtocolVersion(uint8_t id, uint8_t version, const char **log) 218 | { 219 | bool result = false; 220 | 221 | model_name = getModelName(id, log); 222 | if (model_name == NULL) return false; 223 | 224 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 225 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 226 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 227 | !strncmp(model_name, "XM", strlen("XM")) || 228 | !strncmp(model_name, "XL430", strlen("XL430")) || 229 | !strncmp(model_name, "XC430", strlen("XC430")) || 230 | !strncmp(model_name, "XH", strlen("XH"))) 231 | { 232 | result = writeRegister(id, "Protocol_Version", version, log); 233 | if (result == false) 234 | { 235 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set protocol version!"; 236 | return false; 237 | } 238 | } 239 | 240 | result = setPacketHandler((float)version, log); 241 | 242 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set protocol version!"; 243 | return result; 244 | } 245 | 246 | bool DynamixelWorkbench::itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log) 247 | { 248 | return writeRegister(id, item_name, data, log); 249 | } 250 | 251 | bool DynamixelWorkbench::itemRead(uint8_t id, const char *item_name, int32_t *data, const char **log) 252 | { 253 | return readRegister(id, item_name, data, log); 254 | } 255 | 256 | bool DynamixelWorkbench::led(uint8_t id, int32_t onoff, const char **log) 257 | { 258 | bool result = false; 259 | 260 | result = writeRegister(id, "LED", onoff, log); 261 | if (result == false) 262 | { 263 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to change led status!"; 264 | return false; 265 | } 266 | 267 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to change led status!"; 268 | return result; 269 | } 270 | 271 | bool DynamixelWorkbench::ledOn(uint8_t id, const char **log) 272 | { 273 | bool result = false; 274 | 275 | result = led(id, 1, log); 276 | 277 | return result; 278 | } 279 | 280 | bool DynamixelWorkbench::ledOff(uint8_t id, const char **log) 281 | { 282 | bool result = false; 283 | 284 | result = led(id, 0, log); 285 | 286 | return result; 287 | } 288 | 289 | bool DynamixelWorkbench::setNormalDirection(uint8_t id, const char **log) 290 | { 291 | bool result = false; 292 | int32_t data = 0; 293 | 294 | model_name = getModelName(id, log); 295 | if (model_name == NULL) return false; 296 | 297 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 298 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 299 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 300 | !strncmp(model_name, "XM", strlen("XM")) || 301 | !strncmp(model_name, "XL430", strlen("XL430")) || 302 | !strncmp(model_name, "XC430", strlen("XC430")) || 303 | !strncmp(model_name, "XH", strlen("XH"))) 304 | { 305 | result = readRegister(id, "Drive_Mode", &data, log); 306 | 307 | data = data & 0b00000100; 308 | result = writeRegister(id, "Drive_Mode", data, log); 309 | if (result == false) 310 | { 311 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set normal direction!"; 312 | return false; 313 | } 314 | } 315 | 316 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set normal direction!"; 317 | return result; 318 | } 319 | 320 | bool DynamixelWorkbench::setReverseDirection(uint8_t id, const char **log) 321 | { 322 | bool result = false; 323 | int32_t data = 0; 324 | 325 | model_name = getModelName(id, log); 326 | if (model_name == NULL) return false; 327 | 328 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 329 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 330 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 331 | !strncmp(model_name, "XM", strlen("XM")) || 332 | !strncmp(model_name, "XL430", strlen("XL430")) || 333 | !strncmp(model_name, "XC430", strlen("XC430")) || 334 | !strncmp(model_name, "XH", strlen("XH"))) 335 | { 336 | result = readRegister(id, "Drive_Mode", &data, log); 337 | 338 | data = data | 0b00000001; 339 | result = writeRegister(id, "Drive_Mode", data, log); 340 | if (result == false) 341 | { 342 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set reverse direction!"; 343 | return false; 344 | } 345 | } 346 | 347 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set reverse direction!"; 348 | return result; 349 | } 350 | 351 | bool DynamixelWorkbench::setVelocityBasedProfile(uint8_t id, const char **log) 352 | { 353 | bool result = false; 354 | int32_t data = 0; 355 | 356 | model_name = getModelName(id, log); 357 | if (model_name == NULL) return false; 358 | 359 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 360 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 361 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 362 | !strncmp(model_name, "XM", strlen("XM")) || 363 | !strncmp(model_name, "XL430", strlen("XL430")) || 364 | !strncmp(model_name, "XC430", strlen("XC430")) || 365 | !strncmp(model_name, "XH", strlen("XH"))) 366 | { 367 | result = readRegister(id, "Drive_Mode", &data, log); 368 | 369 | data = data & 0b00000001; 370 | result = writeRegister(id, "Drive_Mode", data, log); 371 | if (result == false) 372 | { 373 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set velocity based profile!"; 374 | return false; 375 | } 376 | } 377 | 378 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set velocity based profile!"; 379 | return result; 380 | } 381 | 382 | bool DynamixelWorkbench::setTimeBasedProfile(uint8_t id, const char **log) 383 | { 384 | bool result = false; 385 | int32_t data = 0; 386 | 387 | model_name = getModelName(id, log); 388 | if (model_name == NULL) return false; 389 | 390 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 391 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 392 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 393 | !strncmp(model_name, "XM", strlen("XM")) || 394 | !strncmp(model_name, "XL430", strlen("XL430")) || 395 | !strncmp(model_name, "XC430", strlen("XC430")) || 396 | !strncmp(model_name, "XH", strlen("XH"))) 397 | { 398 | result = readRegister(id, "Drive_Mode", &data, log); 399 | 400 | data = data | 0b00000100; 401 | result = writeRegister(id, "Drive_Mode", data, log); 402 | if (result == false) 403 | { 404 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set time based profile!"; 405 | return false; 406 | } 407 | } 408 | 409 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set time based profile!"; 410 | return result; 411 | } 412 | 413 | bool DynamixelWorkbench::setSecondaryID(uint8_t id, uint8_t secondary_id, const char **log) 414 | { 415 | bool result = false; 416 | 417 | model_name = getModelName(id, log); 418 | if (model_name == NULL) return false; 419 | 420 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 421 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 422 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 423 | !strncmp(model_name, "XM", strlen("XM")) || 424 | !strncmp(model_name, "XL430", strlen("XL430")) || 425 | !strncmp(model_name, "XC430", strlen("XC430")) || 426 | !strncmp(model_name, "XH", strlen("XH")) || 427 | !strncmp(model_name, "RH", strlen("RH"))) 428 | { 429 | result = torqueOff(id, log); 430 | if (result == false) return false; 431 | 432 | result = writeRegister(id, "Secondary_ID", secondary_id, log); 433 | if (result == false) 434 | { 435 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set secondary ID!"; 436 | return false; 437 | } 438 | } 439 | 440 | // millis(1000); 441 | 442 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set secondary ID!"; 443 | return result; 444 | } 445 | 446 | bool DynamixelWorkbench::setPositionControlMode(uint8_t id, const char **log) 447 | { 448 | bool result = false; 449 | 450 | result = setOperatingMode(id, POSITION_CONTROL_MODE, log); 451 | 452 | if (result == false) 453 | { 454 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Position Control Mode!"; 455 | return false; 456 | } 457 | 458 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Position Control Mode!"; 459 | return result; 460 | } 461 | 462 | bool DynamixelWorkbench::setVelocityControlMode(uint8_t id, const char **log) 463 | { 464 | bool result = false; 465 | 466 | result = setOperatingMode(id, VELOCITY_CONTROL_MODE, log); 467 | 468 | if (result == false) 469 | { 470 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Velocity Control Mode!"; 471 | return false; 472 | } 473 | 474 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Velocity Control Mode!"; 475 | return result; 476 | } 477 | 478 | bool DynamixelWorkbench::setCurrentControlMode(uint8_t id, const char **log) 479 | { 480 | bool result = false; 481 | 482 | result = setOperatingMode(id, CURRENT_CONTROL_MODE, log); 483 | 484 | if (result == false) 485 | { 486 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Control Mode!"; 487 | return false; 488 | } 489 | 490 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Control Mode!"; 491 | return result; 492 | } 493 | 494 | bool DynamixelWorkbench::setTorqueControlMode(uint8_t id, const char **log) 495 | { 496 | bool result = false; 497 | 498 | result = setOperatingMode(id, TORQUE_CONTROL_MODE, log); 499 | 500 | if (result == false) 501 | { 502 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Torque Control Mode!"; 503 | return false; 504 | } 505 | 506 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Torque Control Mode!"; 507 | return result; 508 | } 509 | 510 | bool DynamixelWorkbench::setExtendedPositionControlMode(uint8_t id, const char **log) 511 | { 512 | bool result = false; 513 | 514 | result = setOperatingMode(id, EXTENDED_POSITION_CONTROL_MODE, log); 515 | 516 | if (result == false) 517 | { 518 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Extended Position Control Mode!"; 519 | return false; 520 | } 521 | 522 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Extended Position Control Mode!"; 523 | return result; 524 | } 525 | 526 | bool DynamixelWorkbench::setMultiTurnControlMode(uint8_t id, const char **log) 527 | { 528 | bool result = false; 529 | 530 | result = setOperatingMode(id, MULTI_TURN_MODE, log); 531 | 532 | if (result == false) 533 | { 534 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Multi-Turn Control Mode!"; 535 | return false; 536 | } 537 | 538 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Multi-Turn Control Mode!"; 539 | return result; 540 | } 541 | 542 | bool DynamixelWorkbench::setCurrentBasedPositionControlMode(uint8_t id, const char **log) 543 | { 544 | bool result = false; 545 | 546 | result = setOperatingMode(id, CURRENT_BASED_POSITION_CONTROL_MODE, log); 547 | 548 | if (result == false) 549 | { 550 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Based Position Control Mode!"; 551 | return false; 552 | } 553 | 554 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Based Position Control Mode!"; 555 | return result; 556 | } 557 | 558 | bool DynamixelWorkbench::setPWMControlMode(uint8_t id, const char **log) 559 | { 560 | bool result = false; 561 | 562 | result = setOperatingMode(id, PWM_CONTROL_MODE, log); 563 | 564 | if (result == false) 565 | { 566 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set PWM Control Mode!"; 567 | return false; 568 | } 569 | 570 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set PWM Control Mode!"; 571 | return result; 572 | } 573 | 574 | bool DynamixelWorkbench::setOperatingMode(uint8_t id, uint8_t index, const char **log) 575 | { 576 | bool result = false; 577 | 578 | model_name = getModelName(id, log); 579 | if (model_name == NULL) return false; 580 | 581 | if (getProtocolVersion() == 1.0) 582 | { 583 | if (index == POSITION_CONTROL_MODE) 584 | { 585 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 586 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 587 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 588 | !strncmp(model_name, "XL430", strlen("XL430")) || 589 | !strncmp(model_name, "XC430", strlen("XC430")) || 590 | !strncmp(model_name, "XM", strlen("XM")) || 591 | !strncmp(model_name, "XH", strlen("XH")) || 592 | !strncmp(model_name, "PRO", strlen("PRO")) || 593 | !strncmp(model_name, "RH", strlen("RH"))) 594 | { 595 | result = writeRegister(id, "Operating_Mode", POSITION_CONTROL_MODE, log); 596 | } 597 | else if (!strncmp(model_name, "AX", 2) || !strncmp(model_name, "RX", 2)) 598 | { 599 | result = writeRegister(id, "CW_Angle_Limit", 0, log); 600 | result = writeRegister(id, "CCW_Angle_Limit", 1023, log); 601 | } 602 | else 603 | { 604 | result = writeRegister(id, "CW_Angle_Limit", 0, log); 605 | result = writeRegister(id, "CCW_Angle_Limit", 4095, log); 606 | } 607 | } 608 | else if (index == VELOCITY_CONTROL_MODE) 609 | { 610 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 611 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 612 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 613 | !strncmp(model_name, "XL430", strlen("XL430")) || 614 | !strncmp(model_name, "XC430", strlen("XC430")) || 615 | !strncmp(model_name, "XM", strlen("XM")) || 616 | !strncmp(model_name, "XH", strlen("XH")) || 617 | !strncmp(model_name, "PRO", strlen("PRO"))) 618 | { 619 | result = writeRegister(id, "Operating_Mode", VELOCITY_CONTROL_MODE); 620 | } 621 | else 622 | { 623 | result = writeRegister(id, "CW_Angle_Limit", 0, log); 624 | result = writeRegister(id, "CCW_Angle_Limit", 0, log); 625 | } 626 | } 627 | else if (index == CURRENT_CONTROL_MODE) 628 | { 629 | if (!strncmp(model_name, "XM", strlen("XM")) || 630 | !strncmp(model_name, "XH", strlen("XH")) || 631 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 632 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 633 | !strncmp(model_name, "RH", strlen("RH"))) 634 | { 635 | result = writeRegister(id, "Operating_Mode", CURRENT_CONTROL_MODE, log); 636 | } 637 | } 638 | else if (index == TORQUE_CONTROL_MODE) 639 | { 640 | if (!strncmp(model_name, "MX-64", strlen("MX-64")) || 641 | !strncmp(model_name, "MX-106", strlen("MX-106")) ) 642 | { 643 | result = writeRegister(id, "Torque_Control_Mode_Enable", 1, log); 644 | } 645 | } 646 | else if (index == MULTI_TURN_MODE) 647 | { 648 | if (!strncmp(model_name, "MX-64", strlen("MX-64")) || 649 | !strncmp(model_name, "MX-106", strlen("MX-106")) ) 650 | { 651 | result = writeRegister(id, "CW_Angle_Limit", 4095, log); 652 | result = writeRegister(id, "CCW_Angle_Limit", 4095, log); 653 | } 654 | } 655 | else if (index == CURRENT_BASED_POSITION_CONTROL_MODE) 656 | { 657 | if (!strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 658 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 659 | !strncmp(model_name, "XM", strlen("XM")) || 660 | !strncmp(model_name, "XH", strlen("XH")) || 661 | !strncmp(model_name, "RH", strlen("RH"))) 662 | { 663 | result = writeRegister(id, "Operating_Mode", CURRENT_BASED_POSITION_CONTROL_MODE, log); 664 | } 665 | } 666 | else if (index == PWM_CONTROL_MODE) 667 | { 668 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 669 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 670 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 671 | !strncmp(model_name, "XM", strlen("XM")) || 672 | !strncmp(model_name, "XH", strlen("XH"))) 673 | { 674 | result = writeRegister(id, "Operating_Mode", PWM_CONTROL_MODE, log); 675 | } 676 | } 677 | } 678 | else if (getProtocolVersion() == 2.0) 679 | { 680 | if (index == POSITION_CONTROL_MODE) 681 | { 682 | if (!strncmp(model_name, "XL-320", strlen("XL-320"))) 683 | result = writeRegister(id, "Control_Mode", JOINT_MODE, log); 684 | else 685 | result = writeRegister(id, "Operating_Mode", POSITION_CONTROL_MODE, log); 686 | } 687 | else if (index == VELOCITY_CONTROL_MODE) 688 | { 689 | if (!strncmp(model_name, "XL-320", strlen("XL-320"))) 690 | result = writeRegister(id, "Control_Mode", WHEEL_MODE, log); 691 | else 692 | result = writeRegister(id, "Operating_Mode", VELOCITY_CONTROL_MODE, log); 693 | } 694 | else if (index == CURRENT_CONTROL_MODE) 695 | { 696 | if (!strncmp(model_name, "XM", strlen("XM")) || 697 | !strncmp(model_name, "XH", strlen("XH")) || 698 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 699 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 700 | !strncmp(model_name, "RH", strlen("RH"))) 701 | { 702 | result = writeRegister(id, "Operating_Mode", CURRENT_CONTROL_MODE, log); 703 | } 704 | } 705 | else if (index == TORQUE_CONTROL_MODE) 706 | { 707 | if (!strncmp(model_name, "PRO", strlen("PRO")) || 708 | strncmp(model_name, "PRO-L42", strlen("PRO-L42")) ) 709 | { 710 | result = writeRegister(id, "Operating_Mode", 0, log); 711 | } 712 | } 713 | else if (index == EXTENDED_POSITION_CONTROL_MODE) 714 | { 715 | if (!strncmp(model_name, "PRO", strlen("PRO")) || 716 | strncmp(model_name, "PRO-L42", strlen("PRO-L42")) ) 717 | { 718 | result = writeRegister(id, "Operating_Mode", EXTENDED_POSITION_CONTROL_MODE, log); 719 | } 720 | } 721 | else if (index == CURRENT_BASED_POSITION_CONTROL_MODE) 722 | { 723 | if (!strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 724 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 725 | !strncmp(model_name, "XM", strlen("XM")) || 726 | !strncmp(model_name, "XH", strlen("XH")) || 727 | !strncmp(model_name, "RH", strlen("RH"))) 728 | { 729 | result = writeRegister(id, "Operating_Mode", CURRENT_BASED_POSITION_CONTROL_MODE, log); 730 | } 731 | } 732 | else if (index == PWM_CONTROL_MODE) 733 | { 734 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 735 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 736 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 737 | !strncmp(model_name, "XM", strlen("XM")) || 738 | !strncmp(model_name, "XH", strlen("XH")) || 739 | !strncmp(model_name, "XL430", strlen("XL430")) || 740 | !strncmp(model_name, "XC430", strlen("XC430")) || 741 | !strncmp(model_name, "PRO", strlen("PRO"))) 742 | { 743 | result = writeRegister(id, "Operating_Mode", PWM_CONTROL_MODE, log); 744 | } 745 | } 746 | } 747 | 748 | if (result == false) 749 | { 750 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Operating Mode!"; 751 | return false; 752 | } 753 | 754 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Operating Mode!"; 755 | return result; 756 | } 757 | 758 | 759 | bool DynamixelWorkbench::jointMode(uint8_t id, int32_t velocity, int32_t acceleration, const char **log) 760 | { 761 | bool result = false; 762 | 763 | model_name = getModelName(id, log); 764 | if (model_name == NULL) return false; 765 | 766 | result = torqueOff(id, log); 767 | if (result == false) return false; 768 | 769 | result = setPositionControlMode(id, log); 770 | if (result == false) return false; 771 | 772 | if (getProtocolVersion() == 1.0) 773 | { 774 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 775 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 776 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 777 | !strncmp(model_name, "XL430", strlen("XL430")) || 778 | !strncmp(model_name, "XC430", strlen("XC430")) || 779 | !strncmp(model_name, "XM", strlen("XM")) || 780 | !strncmp(model_name, "XH", strlen("XH"))) 781 | { 782 | result = writeRegister(id, "Profile_Acceleration", acceleration, log); 783 | result = writeRegister(id, "Profile_Velocity", velocity, log); 784 | } 785 | else if (!strncmp(model_name, "MX-28", strlen("MX-28")) || 786 | !strncmp(model_name, "MX-64", strlen("MX-64")) || 787 | !strncmp(model_name, "MX-106", strlen("MX-106"))) 788 | { 789 | result = writeRegister(id, "Moving_Speed", velocity, log); 790 | result = writeRegister(id, "Goal_Acceleration", acceleration, log); 791 | } 792 | else 793 | { 794 | result = writeRegister(id, "Moving_Speed", velocity, log); 795 | } 796 | } 797 | else if (getProtocolVersion() == 2.0) 798 | { 799 | if (!strncmp(model_name, "XL-320", strlen("XL-320"))) 800 | { 801 | result = writeRegister(id, "Moving_Speed", velocity, log); 802 | } 803 | else if (!strncmp(model_name, "PRO-M42-10-S260-R-A", strlen("PRO-M42-10-S260-R-A")) || 804 | !strncmp(model_name, "PRO-M54-40-S250-R-A", strlen("PRO-M54-40-S250-R-A")) || 805 | !strncmp(model_name, "PRO-M54-60-S250-R-A", strlen("PRO-M54-60-S250-R-A")) || 806 | !strncmp(model_name, "PRO-H42-20-S300-R-A", strlen("PRO-H42-20-S300-R-A")) || 807 | !strncmp(model_name, "PRO-H54-100-S500-R-A", strlen("PRO-H54-100-S500-R-A")) || 808 | !strncmp(model_name, "PRO-H54-200-S500-R-A", strlen("PRO-H54-200-S500-R-A"))) 809 | { 810 | result = writeRegister(id, "Profile_Acceleration", acceleration, log); 811 | result = writeRegister(id, "Profile_Velocity", velocity, log); 812 | } 813 | else if (!strncmp(model_name, "PRO-L", strlen("PRO-L")) || 814 | !strncmp(model_name, "PRO-M", strlen("PRO-M")) || 815 | !strncmp(model_name, "PRO-H", strlen("PRO-H"))) 816 | { 817 | result = writeRegister(id, "Goal_Velocity", velocity, log); 818 | result = writeRegister(id, "Goal_Acceleration", acceleration, log); 819 | } 820 | else 821 | { 822 | result = writeRegister(id, "Profile_Acceleration", acceleration, log); 823 | result = writeRegister(id, "Profile_Velocity", velocity, log); 824 | } 825 | } 826 | 827 | if (result == false) 828 | { 829 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Joint Mode!"; 830 | return false; 831 | } 832 | 833 | result = torqueOn(id, log); 834 | if (result == false) return false; 835 | 836 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Joint Mode!"; 837 | return result; 838 | } 839 | 840 | bool DynamixelWorkbench::wheelMode(uint8_t id, int32_t acceleration, const char **log) 841 | { 842 | bool result = false; 843 | 844 | model_name = getModelName(id, log); 845 | if (model_name == NULL) return false; 846 | 847 | result = torqueOff(id, log); 848 | if (result == false) return false; 849 | 850 | result = setVelocityControlMode(id, log); 851 | if (result == false) return false; 852 | 853 | if (getProtocolVersion() == 1.0) 854 | { 855 | if (!strncmp(model_name, "MX-28-2", strlen("MX-28-2")) || 856 | !strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 857 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 858 | !strncmp(model_name, "XL430", strlen("XL430")) || 859 | !strncmp(model_name, "XC430", strlen("XC430")) || 860 | !strncmp(model_name, "XM", strlen("XM")) || 861 | !strncmp(model_name, "XH", strlen("XH"))) 862 | { 863 | result = writeRegister(id, "Profile_Acceleration", acceleration, log); 864 | } 865 | else if (!strncmp(model_name, "MX-28", strlen("MX-28")) || 866 | !strncmp(model_name, "MX-64", strlen("MX-64")) || 867 | !strncmp(model_name, "MX-106", strlen("MX-106"))) 868 | { 869 | result = writeRegister(id, "Goal_Acceleration", acceleration, log); 870 | } 871 | } 872 | else if (getProtocolVersion() == 2.0) 873 | { 874 | if (!strncmp(model_name, "PRO-PLUS", strlen("PRO-PLUS"))) 875 | { 876 | result = writeRegister(id, "Profile_Acceleration", acceleration, log); 877 | } 878 | else if (!strncmp(model_name, "PRO", strlen("PRO"))) 879 | { 880 | result = writeRegister(id, "Goal_Acceleration", acceleration, log); 881 | } 882 | else 883 | { 884 | result = writeRegister(id, "Profile_Acceleration", acceleration, log); 885 | } 886 | } 887 | 888 | if (result == false) 889 | { 890 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Wheel Mode!"; 891 | return false; 892 | } 893 | 894 | result = torqueOn(id, log); 895 | if (result == false) return false; 896 | 897 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Wheel Mode!"; 898 | return result; 899 | } 900 | 901 | bool DynamixelWorkbench::currentBasedPositionMode(uint8_t id, int32_t current, const char **log) 902 | { 903 | bool result = false; 904 | 905 | model_name = getModelName(id, log); 906 | if (model_name == NULL) return false; 907 | 908 | result = torqueOff(id, log); 909 | if (result == false) return false; 910 | 911 | result = setCurrentBasedPositionControlMode(id, log); 912 | if (result == false) return false; 913 | 914 | if (!strncmp(model_name, "MX-64-2", strlen("MX-64-2")) || 915 | !strncmp(model_name, "MX-106-2", strlen("MX-106-2")) || 916 | !strncmp(model_name, "XM", strlen("XM")) || 917 | !strncmp(model_name, "XH", strlen("XH")) || 918 | !strncmp(model_name, "RH", strlen("RH"))) 919 | { 920 | result = writeRegister(id, "Goal_Current", current, log); 921 | } 922 | 923 | if (result == false) 924 | { 925 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set Current Based Position Mode!"; 926 | return false; 927 | } 928 | 929 | result = torqueOn(id, log); 930 | if (result == false) return false; 931 | 932 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set Current Based Position Wheel Mode!"; 933 | return result; 934 | } 935 | 936 | //keep compatibility with older codes 937 | bool DynamixelWorkbench::goalPosition(uint8_t id, int value, const char **log) 938 | { 939 | // goalPosition(id, (int32_t)value, log); 940 | // } 941 | 942 | // bool DynamixelWorkbench::goalPosition(uint8_t id, int32_t value, const char **log) 943 | // { 944 | bool result = false; 945 | 946 | result = itemWrite(id, "Goal_Position", value, log); 947 | 948 | if (result == false) 949 | { 950 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal position!"; 951 | return false; 952 | } 953 | 954 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal position!"; 955 | return result; 956 | } 957 | 958 | //keep compatibility with older codes 959 | bool DynamixelWorkbench::goalSpeed(uint8_t id, int value, const char **log) 960 | { 961 | bool result = false; 962 | result = goalVelocity(id, (int32_t)value, log); 963 | return result; 964 | } 965 | 966 | //keep compatibility with older codes 967 | bool DynamixelWorkbench::goalVelocity(uint8_t id, int value, const char **log) 968 | { 969 | // goalVelocity(id, (int32_t)value, log); 970 | // } 971 | 972 | // bool DynamixelWorkbench::goalVelocity(uint8_t id, int32_t value, const char **log) 973 | // { 974 | bool result[2] = {false, false}; 975 | 976 | if (getProtocolVersion() == 2.0f) 977 | { 978 | result[0] = writeRegister(id, "Goal_Velocity", value, log); 979 | if (result[0] == false) 980 | { 981 | if (value < 0) 982 | { 983 | value = (-1) * value; 984 | value |= 1024; 985 | } 986 | result[1] = writeRegister(id, "Moving_Speed", value, log); 987 | if (result[1] == false) 988 | { 989 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!"; 990 | return false; 991 | } 992 | else 993 | { 994 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; 995 | return true; 996 | } 997 | } 998 | else 999 | { 1000 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; 1001 | return true; 1002 | } 1003 | } 1004 | else 1005 | { 1006 | result[0] = writeRegister(id, "Goal_Velocity", value, log); 1007 | if (result[0] == false) 1008 | { 1009 | if (value < 0) 1010 | { 1011 | value = (-1) * value; 1012 | value |= 1024; 1013 | } 1014 | result[1] = writeRegister(id, "Moving_Speed", value, log); 1015 | if (result[1] == false) 1016 | { 1017 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!"; 1018 | return false; 1019 | } 1020 | else 1021 | { 1022 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; 1023 | return true; 1024 | } 1025 | } 1026 | else 1027 | { 1028 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; 1029 | return true; 1030 | } 1031 | } 1032 | 1033 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!"; 1034 | return false; 1035 | } 1036 | 1037 | bool DynamixelWorkbench::goalPosition(uint8_t id, float radian, const char **log) 1038 | { 1039 | bool result = 0; 1040 | uint32_t value = 0; 1041 | 1042 | value = convertRadian2Value(id, radian); 1043 | 1044 | result = goalPosition(id, (int32_t)value, log); 1045 | if (result == false) 1046 | { 1047 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal position!"; 1048 | return false; 1049 | } 1050 | 1051 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal position!"; 1052 | return true; 1053 | } 1054 | 1055 | bool DynamixelWorkbench::goalVelocity(uint8_t id, float velocity, const char **log) 1056 | { 1057 | bool result = 0; 1058 | int32_t value = 0; 1059 | 1060 | value = convertVelocity2Value(id, velocity); 1061 | 1062 | result = goalVelocity(id, value, log); 1063 | if (result == false) 1064 | { 1065 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to set goal velocity!"; 1066 | return result; 1067 | } 1068 | 1069 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to set goal velocity!"; 1070 | return result; 1071 | } 1072 | 1073 | bool DynamixelWorkbench::getPresentPositionData(uint8_t id, int32_t* data, const char **log) 1074 | { 1075 | bool result = 0; 1076 | int32_t get_data = 0; 1077 | 1078 | result = readRegister(id, "Present_Position", &get_data, log); 1079 | if (result == false) 1080 | { 1081 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to get present position data!"; 1082 | return result; 1083 | } 1084 | 1085 | *data = get_data; 1086 | 1087 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get present position data!"; 1088 | return result; 1089 | } 1090 | 1091 | bool DynamixelWorkbench::getRadian(uint8_t id, float* radian, const char **log) 1092 | { 1093 | bool result = 0; 1094 | int32_t get_data = 0; 1095 | 1096 | result = getPresentPositionData(id, &get_data, log); 1097 | if (result == false) 1098 | { 1099 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to get radian!"; 1100 | return result; 1101 | } 1102 | 1103 | *radian = convertValue2Radian(id, get_data); 1104 | 1105 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get radian!"; 1106 | return result; 1107 | } 1108 | 1109 | bool DynamixelWorkbench::getVelocity(uint8_t id, float* velocity, const char **log) 1110 | { 1111 | bool result = 0; 1112 | int32_t get_data = 0; 1113 | 1114 | result = getPresentVelocityData(id, &get_data, log); 1115 | if (result == false) 1116 | { 1117 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to get velocity!"; 1118 | return result; 1119 | } 1120 | 1121 | *velocity = convertValue2Velocity(id, get_data); 1122 | 1123 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get velocity!"; 1124 | return result; 1125 | } 1126 | 1127 | bool DynamixelWorkbench::getPresentVelocityData(uint8_t id, int32_t* data, const char **log) 1128 | { 1129 | bool result[2] = {false, false}; 1130 | int32_t get_data = 0; 1131 | 1132 | result[0] = readRegister(id, "Goal_Velocity", &get_data, log); 1133 | if (result[0] == false) 1134 | { 1135 | result[1] = readRegister(id, "Moving_Speed", &get_data, log); 1136 | if (result[1] == false) 1137 | { 1138 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to get goal velocity!"; 1139 | return false; 1140 | } 1141 | else 1142 | { 1143 | *data = get_data; 1144 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get goal velocity!"; 1145 | return true; 1146 | } 1147 | } 1148 | else 1149 | { 1150 | *data = get_data; 1151 | if (log != NULL) *log = "[DynamixelWorkbench] Succeeded to get goal velocity!"; 1152 | return true; 1153 | } 1154 | 1155 | if (log != NULL) *log = "[DynamixelWorkbench] Failed to get goal velocity!"; 1156 | return false; 1157 | } 1158 | 1159 | int32_t DynamixelWorkbench::convertRadian2Value(uint8_t id, float radian) 1160 | { 1161 | int32_t position = 0; 1162 | 1163 | model_info = getModelInfo(id); 1164 | if (model_info == NULL) return false; 1165 | 1166 | if (radian > 0) 1167 | { 1168 | position = (radian * (model_info->value_of_max_radian_position - model_info->value_of_zero_radian_position) / model_info->max_radian) + model_info->value_of_zero_radian_position; 1169 | } 1170 | else if (radian < 0) 1171 | { 1172 | position = (radian * (model_info->value_of_min_radian_position - model_info->value_of_zero_radian_position) / model_info->min_radian) + model_info->value_of_zero_radian_position; 1173 | } 1174 | else 1175 | { 1176 | position = model_info->value_of_zero_radian_position; 1177 | } 1178 | 1179 | return position; 1180 | } 1181 | 1182 | float DynamixelWorkbench::convertValue2Radian(uint8_t id, int32_t value) 1183 | { 1184 | float radian = 0.0; 1185 | 1186 | model_info = getModelInfo(id); 1187 | if (model_info == NULL) return false; 1188 | 1189 | if (value > model_info->value_of_zero_radian_position) 1190 | { 1191 | radian = (float)(value - model_info->value_of_zero_radian_position) * model_info->max_radian / (float)(model_info->value_of_max_radian_position - model_info->value_of_zero_radian_position); 1192 | } 1193 | else if (value < model_info->value_of_zero_radian_position) 1194 | { 1195 | radian = (float)(value - model_info->value_of_zero_radian_position) * model_info->min_radian / (float)(model_info->value_of_min_radian_position - model_info->value_of_zero_radian_position); 1196 | } 1197 | 1198 | return radian; 1199 | } 1200 | 1201 | int32_t DynamixelWorkbench::convertRadian2Value(float radian, int32_t max_position, int32_t min_position, float max_radian, float min_radian) 1202 | { 1203 | int32_t value = 0; 1204 | int32_t zero_position = (max_position + min_position)/2; 1205 | 1206 | if (radian > 0) 1207 | { 1208 | value = (radian * (max_position - zero_position) / max_radian) + zero_position; 1209 | } 1210 | else if (radian < 0) 1211 | { 1212 | value = (radian * (min_position - zero_position) / min_radian) + zero_position; 1213 | } 1214 | else 1215 | { 1216 | value = zero_position; 1217 | } 1218 | 1219 | return value; 1220 | } 1221 | 1222 | float DynamixelWorkbench::convertValue2Radian(int32_t value, int32_t max_position, int32_t min_position, float max_radian, float min_radian) 1223 | { 1224 | float radian = 0.0; 1225 | int32_t zero_position = (max_position + min_position)/2; 1226 | 1227 | if (value > zero_position) 1228 | { 1229 | radian = (float)(value - zero_position) * max_radian / (float)(max_position - zero_position); 1230 | } 1231 | else if (value < zero_position) 1232 | { 1233 | radian = (float)(value - zero_position) * min_radian / (float)(min_position - zero_position); 1234 | } 1235 | 1236 | return radian; 1237 | } 1238 | 1239 | int32_t DynamixelWorkbench::convertVelocity2Value(uint8_t id, float velocity) 1240 | { 1241 | int32_t value = 0; 1242 | const float RPM2RADPERSEC = 0.104719755f; 1243 | 1244 | model_info = getModelInfo(id); 1245 | if (model_info == NULL) return false; 1246 | 1247 | if (getProtocolVersion() == 1.0f) 1248 | { 1249 | if (strncmp(getModelName(id), "AX", strlen("AX")) == 0 || 1250 | strncmp(getModelName(id), "RX", strlen("RX")) == 0 || 1251 | strncmp(getModelName(id), "EX", strlen("EX")) == 0 || 1252 | strncmp(getModelName(id), "MX", strlen("MX")) == 0) 1253 | { 1254 | if (velocity == 0.0f) value = 0; 1255 | else if (velocity < 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)); 1256 | else if (velocity > 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)) + 1023; 1257 | 1258 | return value; 1259 | } 1260 | } 1261 | else if (getProtocolVersion() == 2.0f) 1262 | { 1263 | if (strcmp(getModelName(id), "XL-320") == 0) 1264 | { 1265 | if (velocity == 0.0f) value = 0; 1266 | else if (velocity < 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)); 1267 | else if (velocity > 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)) + 1023; 1268 | 1269 | return value; 1270 | } 1271 | else 1272 | { 1273 | value = velocity / (model_info->rpm * RPM2RADPERSEC); 1274 | } 1275 | 1276 | return value; 1277 | } 1278 | 1279 | return 0; 1280 | } 1281 | 1282 | float DynamixelWorkbench::convertValue2Velocity(uint8_t id, int32_t value) 1283 | { 1284 | float velocity = 0; 1285 | const float RPM2RADPERSEC = 0.104719755f; 1286 | 1287 | model_info = getModelInfo(id); 1288 | if (model_info == NULL) return false; 1289 | 1290 | if (getProtocolVersion() == 1.0f) 1291 | { 1292 | if (strncmp(getModelName(id), "AX", strlen("AX")) == 0 || 1293 | strncmp(getModelName(id), "RX", strlen("RX")) == 0 || 1294 | strncmp(getModelName(id), "EX", strlen("EX")) == 0 || 1295 | strncmp(getModelName(id), "MX", strlen("MX")) == 0) 1296 | { 1297 | if (value == 1023 || value == 0) velocity = 0.0f; 1298 | else if (value > 0 && value < 1023) velocity = value * model_info->rpm * RPM2RADPERSEC; 1299 | else if (value > 1023 && value < 2048) velocity = (value - 1023) * model_info->rpm * RPM2RADPERSEC * (-1.0f); 1300 | 1301 | return velocity; 1302 | } 1303 | } 1304 | else if (getProtocolVersion() == 2.0f) 1305 | { 1306 | if (strcmp(getModelName(id), "XL-320") == 0) 1307 | { 1308 | if (value == 1023 || value == 0) velocity = 0.0f; 1309 | else if (value > 0 && value < 1023) velocity = value * model_info->rpm * RPM2RADPERSEC; 1310 | else if (value > 1023 && value < 2048) velocity = (value - 1023) * model_info->rpm * RPM2RADPERSEC * (-1.0f); 1311 | } 1312 | else 1313 | { 1314 | velocity = value * (model_info->rpm * RPM2RADPERSEC); 1315 | } 1316 | 1317 | return velocity; 1318 | } 1319 | 1320 | return 0.0f; 1321 | } 1322 | 1323 | int16_t DynamixelWorkbench::convertCurrent2Value(uint8_t id, float current) 1324 | { 1325 | float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102 1326 | 1327 | model_info = getModelInfo(id); 1328 | if (model_info == NULL) return false; 1329 | 1330 | if (getProtocolVersion() == 1.0f) 1331 | { 1332 | return (current / CURRENT_UNIT); 1333 | } 1334 | else if (getProtocolVersion() == 2.0f) 1335 | { 1336 | if (strncmp(getModelName(id), "PRO-L", strlen("PRO-L")) == 0 || 1337 | strncmp(getModelName(id), "PRO-M", strlen("PRO-M")) == 0 || 1338 | strncmp(getModelName(id), "PRO-H", strlen("PRO-H")) == 0) 1339 | { 1340 | CURRENT_UNIT = 16.11328f; 1341 | return (current / CURRENT_UNIT); 1342 | } 1343 | else if (strncmp(getModelName(id), "PRO-PLUS", strlen("PRO-PLUS")) == 0) 1344 | { 1345 | CURRENT_UNIT = 1.0f; 1346 | return (current / CURRENT_UNIT); 1347 | } 1348 | else 1349 | { 1350 | return (current / CURRENT_UNIT); 1351 | } 1352 | } 1353 | 1354 | return (current / CURRENT_UNIT); 1355 | } 1356 | 1357 | int16_t DynamixelWorkbench::convertCurrent2Value(float current) 1358 | { 1359 | int16_t value = 0; 1360 | const float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102 1361 | 1362 | value = current / CURRENT_UNIT; 1363 | 1364 | return value; 1365 | } 1366 | 1367 | float DynamixelWorkbench::convertValue2Current(uint8_t id, int16_t value) 1368 | { 1369 | float current = 0; 1370 | float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102 1371 | 1372 | model_info = getModelInfo(id); 1373 | if (model_info == NULL) return false; 1374 | 1375 | if (getProtocolVersion() == 1.0f) 1376 | { 1377 | current = (int16_t)value * CURRENT_UNIT; 1378 | return current; 1379 | } 1380 | else if (getProtocolVersion() == 2.0f) 1381 | { 1382 | if (strncmp(getModelName(id), "PRO-L", strlen("PRO-L")) == 0 || 1383 | strncmp(getModelName(id), "PRO-M", strlen("PRO-M")) == 0 || 1384 | strncmp(getModelName(id), "PRO-H", strlen("PRO-H")) == 0) 1385 | { 1386 | CURRENT_UNIT = 16.11328f; 1387 | current = (int16_t)value * CURRENT_UNIT; 1388 | return current; 1389 | } 1390 | else if (strncmp(getModelName(id), "PRO-PLUS", strlen("PRO-PLUS")) == 0) 1391 | { 1392 | CURRENT_UNIT = 1.0f; 1393 | current = (int16_t)value * CURRENT_UNIT; 1394 | return current; 1395 | } 1396 | else 1397 | { 1398 | current = (int16_t)value * CURRENT_UNIT; 1399 | return current; 1400 | } 1401 | } 1402 | 1403 | current = (int16_t)value * CURRENT_UNIT; 1404 | 1405 | return current; 1406 | } 1407 | 1408 | float DynamixelWorkbench::convertValue2Current(int16_t value) 1409 | { 1410 | float current = 0; 1411 | const float CURRENT_UNIT = 2.69f; //Unit : mA, Ref : http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/#goal-current102 1412 | 1413 | current = (int16_t)value * CURRENT_UNIT; 1414 | 1415 | return current; 1416 | } 1417 | 1418 | float DynamixelWorkbench::convertValue2Load(int16_t value) 1419 | { 1420 | float load = 0; 1421 | const float LOAD_UNIT = 0.1f; //Unit : %, Ref : http://emanual.robotis.com/docs/en/dxl/mx/mx-28/#present-load 1422 | 1423 | if (value == 1023 || value == 0) load = 0.0f; 1424 | else if (value > 0 && value < 1023) load = value * LOAD_UNIT; 1425 | else if (value > 1023 && value < 2048) load = (value - 1023) * LOAD_UNIT * (-1.0f); 1426 | 1427 | return load; 1428 | } 1429 | --------------------------------------------------------------------------------