diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md index 34d43d6a..154e2b61 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE.md @@ -1,64 +1,24 @@ -ISSUE TEMPLATE ver. 1.1.1 +ISSUE TEMPLATE ver. 1.2.0 +Please fill this template for more accurate and prompt support. +1. Which DYNAMIXEL SDK version do you use? + - ex) `3.4.1` +2. Which programming language/tool do you use? + - ex) `C`, `C++`, `C#`, `Python`, `Java`, `MATLAB`, `LABVIEW`, `ROS`, `Arduino`, etc +3. Which operating system do you use? + - ex) `Ubuntu 16.04`, `Windows 10`, `OS X 10.14` +3. Which USB serial converter do you use? + - ex) `USB2Dynamixel`, `U2D2`, `OpenCM904`, `OpenCR`, etc +4. Which DYNAMIXEL do you use? + - ex) `MX-28(2.0)`, `AX-12W`, `XM430-W250`, etc. +5. Have you searched the issue from the closed issue threads? -BE CAREFUL!! FOLLOW THE RULES AS FOLLOWS, OR YOUR ISSUE WILL BE `WON'T FIX` ANYWAY +6. Please describe the issue in detail - - - - - - - - -STEP 1. Check what you are suffering from : - -- I'm using `DynamixelSDK ver. 3.5.4` - - - [3.4.1], [3.5.2], etc. - -- I'm using `C++` Language - - - [C] / [C++] / [C#] / [Python] / [Java] / [MATLAB] / [LABVIEW] / [ROS] / [Arduino] - -- I'm using `USB2Dynamixel2` serial converter - - - [USB2Dynamixel] / [USB2Dynamixel2] or [OpenCM904] / [OpenCR] / [CM700s] / [CM500s] or other ROBOTIS product only - -- I'm using `XM430-W210-R` - - - [MX-28], [AX-12W], [H54-200W], etc. - -- and I'm having an `issue` like what - - - 'My motor doesn't work', etc. - - -STEP 2. Write `Title` as `[3.5.4][C++][USB2Dynamixel2][XM430-W210-R] Issue : My motor doesn't work` - -- Now, if you think : - - - Hey, I'm using RoboPlus2.0, and having an issue, but how can I write that kind of `Title`? - - - Hey, I'm using USB2AX serial converter, and having an issue, but how can I write that kind of `Title`? - - - or more that is not listed on above. - -- , You should write your `issue` on http://en.robotis.com/BlueAD/board.php?bbs_id=old_qna - - -STEP 3. Delete all written here, and describe what your problem is - -- Any PICTURES or VIDEOS? - - ![](　Link the PICTURES or VIDEOS here, if necessary　) - -- Any SOURCE SAMPLES? - - [](　Link the SOURCE SAMPLES here, if necessary　) +7. How can we reproduce the issue? diff --git a/.travis.yml b/.travis.yml index 69f8dc9c..4fadd455 100644 --- a/.travis.yml +++ b/.travis.yml @@ -15,16 +15,16 @@ notifications: on_success: change on_failure: always recipients: - - pyo@robotis.com + - willson@robotis.com env: matrix: - ROS_DISTRO=indigo ROS_REPO=ros UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=trusty - ROS_DISTRO=kinetic ROS_REPO=ros UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=xenial - ROS_DISTRO=melodic ROS_REPO=ros UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=bionic - ROS_DISTRO=melodic ROS_REPO=ros UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=stretch + - ROS_DISTRO=noetic ROS_REPO=ros UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=focal matrix: allow_failures: - - env: ROS_DISTRO=melodic ROS_REPO=ros UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=bionic - env: ROS_DISTRO=melodic ROS_REPO=ros UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=stretch branches: only: @@ -33,8 +33,8 @@ branches: - indigo-devel - kinetic-devel - melodic-devel + - noetic-devel install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: - source .ci_config/travis.sh - diff --git a/README.md b/README.md index b7f98234..7dac9751 100644 --- a/README.md +++ b/README.md @@ -12,10 +12,15 @@ DynamixelSDK supports various programming languages. - **Python**: Python module and examples (* Dynamic library (*.dll, *.so, and *.dylib files) / .dll: dynamic-link library on Windows / .so: shared object on Linux / .dylib: dynamic library on MacOS) -## ROS Packages for Dynamixel SDK -|Version|Indigo + Ubuntu Trusty|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| -|:---:|:---:|:---:|:---:| -|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FDynamixelSDK.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FDynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=indigo-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=melodic-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)| +## ROS 1 Packages for Dynamixel SDK +|develop|master|Indigo + Ubuntu Trusty|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic|Noetic + Ubuntu Focal| +|:---:|:---:|:---:|:---:|:---:|:---:| +|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=develop)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=indigo-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=melodic-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=noetic-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)| + +## ROS 2 Packages for Dynamixel SDK +|ros2-devel|ros2|Dashing + Ubuntu Bionic|Eloquent + Ubuntu Bionic|Foxy + Ubuntu Foxy| +|:---:|:---:|:---:|:---:|:---:| +|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=ros2-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=ros2)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=dashing-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=eloquent-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK.svg?branch=foxy-devel)](https://travis-ci.org/ROBOTIS-GIT/DynamixelSDK)| For more information on ROS Packages for Dynamixel SDK, please refer to the ROS wiki pages below. - http://wiki.ros.org/dynamixel_sdk diff --git a/ReleaseNote.md b/ReleaseNote.md index c1bb7dfb..93db749a 100644 --- a/ReleaseNote.md +++ b/ReleaseNote.md @@ -1,17 +1,48 @@ # Dynamixel SDK Release Notes +3.7.41(2020-08-12) +------------------- +* DYNAMIXEL SDK ROS example initial commit. +* Bug fix for 4.5Mbps support (#430) +* Contributors: Zerom, Will Son + +3.7.31 (2020-07-13) +------------------- +* ROS 1 Noetic Ninjemys support +* 3x faster getError member function of GroupSyncRead Class +* Contributors: developer0hye, Zerom, Will Son + +3.7.21 (2019-09-06) +------------------- +* Fixed buffer overflow bug (rxpacket size) +* Fixed typo in the package.xml and header files + +3.7.11 (2019-08-19) +------------------- +* Updated C lib and DLL file +* Changed C# / win32 / protocol_combined output path +* Fixed "protocol_combined" example bug +* Fixed typo in bulk_read_write.py + +3.7.0 (2019-01-03) +------------------ +* Added clear instruction [#269](https://github.com/ROBOTIS-GIT/DynamixelSDK/issues/269) +* Removed busy waiting for rxPacket() +* Fixed addStuffing() function (reduced stack memory usage) +* Fixed memory issues [#268](https://github.com/ROBOTIS-GIT/DynamixelSDK/issues/268) +* Fixed the broadcast ping bug in dxl_monitor + 3.6.2 (2018-07-17) ------------------ -* added python modules for ROS to ros folder -* moved cpp library files for ROS to ros folder -* created an ROS package separately `#187 `_ -* modified the e-Manual address to emanual.robotis.com +* Added python modules for ROS to ros folder +* Moved cpp library files for ROS to ros folder +* Created an ROS package separately `#187 `_ +* Modified the e-Manual address to emanual.robotis.com 3.6.1 (2018-06-14) ------------------ -* removed printTxRxResult(), printRxPacketError() `#193 `_ -* removed cache files -* merge pull request `#195 `_ +* Removed printTxRxResult(), printRxPacketError() `#193 `_ +* Removed cache files 3.6.0 (2018-03-16) ------------------ diff --git a/c#/protocol2.0/clear_multi_turn/win32/.vs/clear_multi_turn/v14/.suo b/c#/protocol2.0/clear_multi_turn/win32/.vs/clear_multi_turn/v14/.suo new file mode 100644 index 00000000..6955f04a Binary files /dev/null and b/c#/protocol2.0/clear_multi_turn/win32/.vs/clear_multi_turn/v14/.suo differ diff --git a/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn.sln b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn.sln new file mode 100644 index 00000000..461a2a35 --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn.sln @@ -0,0 +1,34 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 14 +VisualStudioVersion = 14.0.25420.1 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "clear_multi_turn", "clear_multi_turn\clear_multi_turn.csproj", "{321BAE3A-A494-47FA-A8CB-903202EB5FAE}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|Any CPU.ActiveCfg = Release|Any CPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|Any CPU.Build.0 = Release|Any CPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|x64.ActiveCfg = Debug|x64 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|x64.Build.0 = Debug|x64 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|x86.ActiveCfg = Debug|x86 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|x86.Build.0 = Debug|x86 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|Any CPU.ActiveCfg = Release|Any CPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|Any CPU.Build.0 = Release|Any CPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|x64.ActiveCfg = Release|x64 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|x64.Build.0 = Release|x64 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|x86.ActiveCfg = Release|x86 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/App.config b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/App.config new file mode 100644 index 00000000..d1428ad7 --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/App.config @@ -0,0 +1,6 @@ + + + + + + diff --git a/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/ClearMultiTurn.cs b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/ClearMultiTurn.cs new file mode 100644 index 00000000..37e57f3d --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/ClearMultiTurn.cs @@ -0,0 +1,250 @@ +/******************************************************************************* +* Copyright 2017 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Author: Ki Jong Gil (Gilbert) */ + +// +// ********* Clear Multi-Turn Example ********* +// +// +// Available Dynamixel model on this example : Dynamixel X-series (firmware v42 or above) +// This example is designed for using a Dynamixel XM430-W350-R, and an U2D2. +// To use another Dynamixel model, such as MX series, see their details in E-Manual(emanual.robotis.com) and edit below "#define"d variables yourself. +// Be sure that Dynamixel properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +// + +using System; +using System.Runtime.InteropServices; +using dynamixel_sdk; + +namespace clear_multi_turn +{ + class ClearMultiTurn + { + // Control table address + public const int ADDR_OPERATING_MODE = 11; // Control table address is different in Dynamixel model + public const int ADDR_TORQUE_ENABLE = 64; + public const int ADDR_GOAL_POSITION = 116; + public const int ADDR_PRESENT_POSITION = 132; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 57600; + public const string DEVICENAME = "COM1"; // Check which port is being used on your controller + // ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int MAX_POSITION_VALUE = 1048575; // + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + public const int EXT_POSITION_CONTROL_MODE = 4; // Value for extended position control mode (operating mode) + + public const byte ESC_ASCII_VALUE = 0x1b; + public const byte SPACE_ASCII_VALUE = 0x20; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + Int32 dxl_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + else + { + Console.WriteLine("Operating mode changed to extended position control mode."); + } + + // Enable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + else + { + Console.WriteLine("Dynamixel has been successfully connected"); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey(true).KeyChar == ESC_ASCII_VALUE) + break; + + Console.WriteLine(" Press SPACE key to clear multi-turn information! (or press ESC to stop!)"); + + // Write goal position + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, MAX_POSITION_VALUE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + do + { + // Read present position + dxl_present_position = (Int32)dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + Console.Write(string.Format(" [ID: {0}] GoalPos: {1} PresPos: {2}", DXL_ID, MAX_POSITION_VALUE, dxl_present_position, Environment.NewLine)); + Console.Write("\r".PadLeft(Console.WindowWidth - Console.CursorLeft - 1)); + if (Console.KeyAvailable) + { + char c = Console.ReadKey().KeyChar; + if (c == SPACE_ASCII_VALUE) + { + Console.WriteLine("\n Stop & Clear Multi-Turn Information!"); + + // Write the present position to the goal position to stop moving + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, (UInt32)dxl_present_position); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + System.Threading.Thread.Sleep(300); + + // Clear Multi-Turn Information + dynamixel.clearMultiTurn(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + // Read present position + dxl_present_position = (Int32)dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + Console.WriteLine(" Present Position has been reset. : {0} \n", dxl_present_position); + + break; + } + else if ( c == ESC_ASCII_VALUE) + { + Console.WriteLine("\n Stopped!! \n"); + + // Write the present position to the goal position to stop moving + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, (UInt32)dxl_present_position); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + break; + } + } + } while ((Math.Abs(MAX_POSITION_VALUE - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + } + + // Disable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} \ No newline at end of file diff --git a/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/Properties/AssemblyInfo.cs b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..6bea8bcc --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("clear_multi_turn")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("clear_multi_turn")] +[assembly: AssemblyCopyright("Copyright © 2018")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("321bae3a-a494-47fa-a8cb-903202eb5fae")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the values or you can default the Build and Revision Numbers +// by using the '*' as shown below: +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.csproj b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.csproj new file mode 100644 index 00000000..f6ff2408 --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.csproj @@ -0,0 +1,102 @@ + + + + + Debug + AnyCPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE} + Exe + Properties + clear_multi_turn + clear_multi_turn + v4.5 + 512 + true + + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/dynamixel.cs b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/dynamixel.cs new file mode 100644 index 00000000..cc0d12e2 --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win32/clear_multi_turn/dynamixel.cs @@ -0,0 +1,262 @@ +/******************************************************************************* +* Copyright 2017 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Author: Ryu Woon Jung (Leon) */ + +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../../../../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler (string port_name); + + [DllImport(dll_path)] + public static extern bool openPort (int port_num); + [DllImport(dll_path)] + public static extern void closePort (int port_num); + [DllImport(dll_path)] + public static extern void clearPort (int port_num); + + [DllImport(dll_path)] + public static extern void setPortName (int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName (int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate (int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate (int port_num); + + [DllImport(dll_path)] + public static extern int readPort (int port_num, byte[] packet, int length); + [DllImport(dll_path)] + public static extern int writePort (int port_num, byte[] packet, int length); + + [DllImport(dll_path)] + public static extern void setPacketTimeout (int port_num, UInt16 packet_length); + [DllImport(dll_path)] + public static extern void setPacketTimeoutMSec(int port_num, double msec); + [DllImport(dll_path)] + public static extern bool isPacketTimeout (int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler (); + + [DllImport(dll_path)] + public static extern IntPtr getTxRxResult (int protocol_version, int result); + [DllImport(dll_path)] + public static extern IntPtr getRxPacketError (int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite (int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead (int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping (int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum (int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void clearMultiTurn (int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void reboot (int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset (int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx (int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx (int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx (int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx (int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly (int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly (int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly (int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx (int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly (int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam(int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable(int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam (int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket (int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/clear_multi_turn/win64/.vs/clear_multi_turn/v14/.suo b/c#/protocol2.0/clear_multi_turn/win64/.vs/clear_multi_turn/v14/.suo new file mode 100644 index 00000000..3aff87b0 Binary files /dev/null and b/c#/protocol2.0/clear_multi_turn/win64/.vs/clear_multi_turn/v14/.suo differ diff --git a/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn.sln b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn.sln new file mode 100644 index 00000000..c9ff38d4 --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 14 +VisualStudioVersion = 14.0.25420.1 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "clear_multi_turn", "clear_multi_turn\clear_multi_turn.csproj", "{321BAE3A-A494-47FA-A8CB-903202EB5FAE}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|Any CPU.ActiveCfg = Release|Any CPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|Any CPU.Build.0 = Release|Any CPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|x64.ActiveCfg = Debug|x64 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Debug|x64.Build.0 = Debug|x64 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|Any CPU.ActiveCfg = Release|Any CPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|Any CPU.Build.0 = Release|Any CPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|x64.ActiveCfg = Release|x64 + {321BAE3A-A494-47FA-A8CB-903202EB5FAE}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/App.config b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/App.config new file mode 100644 index 00000000..d1428ad7 --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/App.config @@ -0,0 +1,6 @@ + + + + + + diff --git a/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/ClearMultiTurn.cs b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/ClearMultiTurn.cs new file mode 100644 index 00000000..4e699592 --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/ClearMultiTurn.cs @@ -0,0 +1,250 @@ +/******************************************************************************* +* Copyright 2017 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Author: Ki Jong Gil (Gilbert) */ + +// +// ********* Clear Multi-Turn Example ********* +// +// +// Available Dynamixel model on this example : Dynamixel X-series (firmware v42 or above) +// This example is designed for using a Dynamixel XM430-W350-R, and an U2D2. +// To use another Dynamixel model, such as MX series, see their details in E-Manual(emanual.robotis.com) and edit below "#define"d variables yourself. +// Be sure that Dynamixel properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +// + +using System; +using System.Runtime.InteropServices; +using dynamixel_sdk; + +namespace clear_multi_turn +{ + class ClearMultiTurn + { + // Control table address + public const int ADDR_OPERATING_MODE = 11; // Control table address is different in Dynamixel model + public const int ADDR_TORQUE_ENABLE = 64; + public const int ADDR_GOAL_POSITION = 116; + public const int ADDR_PRESENT_POSITION = 132; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 57600; + public const string DEVICENAME = "COM1"; // Check which port is being used on your controller + // ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int MAX_POSITION_VALUE = 1048575; // + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + public const int EXT_POSITION_CONTROL_MODE = 4; // Value for extended position control mode (operating mode) + + public const byte ESC_ASCII_VALUE = 0x1b; + public const byte SPACE_ASCII_VALUE = 0x20; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + Int32 dxl_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + else + { + Console.WriteLine("Operating mode changed to extended position control mode."); + } + + // Enable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + else + { + Console.WriteLine("Dynamixel has been successfully connected"); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey(true).KeyChar == ESC_ASCII_VALUE) + break; + + Console.WriteLine(" Press SPACE key to clear multi-turn information! (or press ESC to stop!)"); + + // Write goal position + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, MAX_POSITION_VALUE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + do + { + // Read present position + dxl_present_position = (Int32)dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + Console.Write(string.Format(" [ID: {0}] GoalPos: {1} PresPos: {2}", DXL_ID, MAX_POSITION_VALUE, dxl_present_position, Environment.NewLine)); + Console.Write("\r".PadLeft(Console.WindowWidth - Console.CursorLeft - 1)); + if (Console.KeyAvailable) + { + char c = Console.ReadKey().KeyChar; + if (c == SPACE_ASCII_VALUE) + { + Console.WriteLine("\n Stop & Clear Multi-Turn Information!"); + + // Write the present position to the goal position to stop moving + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, (UInt32)dxl_present_position); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + System.Threading.Thread.Sleep(300); + + // Clear Multi-Turn Information + dynamixel.clearMultiTurn(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + // Read present position + dxl_present_position = (Int32)dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + Console.WriteLine(" Present Position has been reset. : {0} \n", dxl_present_position); + + break; + } + else if ( c == ESC_ASCII_VALUE) + { + Console.WriteLine("\n Stopped!! \n"); + + // Write the present position to the goal position to stop moving + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, (UInt32)dxl_present_position); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + break; + } + } + } while ((Math.Abs(MAX_POSITION_VALUE - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + } + + // Disable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} \ No newline at end of file diff --git a/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/Properties/AssemblyInfo.cs b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..6bea8bcc --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("clear_multi_turn")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("clear_multi_turn")] +[assembly: AssemblyCopyright("Copyright © 2018")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("321bae3a-a494-47fa-a8cb-903202eb5fae")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the values or you can default the Build and Revision Numbers +// by using the '*' as shown below: +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.csproj b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.csproj new file mode 100644 index 00000000..4c008832 --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.csproj @@ -0,0 +1,82 @@ + + + + + Debug + AnyCPU + {321BAE3A-A494-47FA-A8CB-903202EB5FAE} + Exe + Properties + clear_multi_turn + clear_multi_turn + v4.5 + 512 + true + + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/dynamixel.cs b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/dynamixel.cs new file mode 100644 index 00000000..cc0d12e2 --- /dev/null +++ b/c#/protocol2.0/clear_multi_turn/win64/clear_multi_turn/dynamixel.cs @@ -0,0 +1,262 @@ +/******************************************************************************* +* Copyright 2017 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Author: Ryu Woon Jung (Leon) */ + +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../../../../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler (string port_name); + + [DllImport(dll_path)] + public static extern bool openPort (int port_num); + [DllImport(dll_path)] + public static extern void closePort (int port_num); + [DllImport(dll_path)] + public static extern void clearPort (int port_num); + + [DllImport(dll_path)] + public static extern void setPortName (int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName (int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate (int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate (int port_num); + + [DllImport(dll_path)] + public static extern int readPort (int port_num, byte[] packet, int length); + [DllImport(dll_path)] + public static extern int writePort (int port_num, byte[] packet, int length); + + [DllImport(dll_path)] + public static extern void setPacketTimeout (int port_num, UInt16 packet_length); + [DllImport(dll_path)] + public static extern void setPacketTimeoutMSec(int port_num, double msec); + [DllImport(dll_path)] + public static extern bool isPacketTimeout (int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler (); + + [DllImport(dll_path)] + public static extern IntPtr getTxRxResult (int protocol_version, int result); + [DllImport(dll_path)] + public static extern IntPtr getRxPacketError (int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite (int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead (int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping (int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum (int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void clearMultiTurn (int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void reboot (int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset (int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx (int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx (int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx (int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx (int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx (int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly (int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly (int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly (int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx (int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx (int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly (int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam(int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable(int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam (int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket (int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/read_write/win32/.vs/read_write/v14/.suo b/c#/protocol2.0/read_write/win32/.vs/read_write/v14/.suo index 7b2b4760..a52b9b35 100644 Binary files a/c#/protocol2.0/read_write/win32/.vs/read_write/v14/.suo and b/c#/protocol2.0/read_write/win32/.vs/read_write/v14/.suo differ diff --git a/c#/protocol_combined/win32/protocol_combined/dynamixel.cs b/c#/protocol_combined/win32/protocol_combined/dynamixel.cs index 4b98a146..1edf30dc 100644 --- a/c#/protocol_combined/win32/protocol_combined/dynamixel.cs +++ b/c#/protocol_combined/win32/protocol_combined/dynamixel.cs @@ -23,7 +23,7 @@ namespace dynamixel_sdk { class dynamixel { - const string dll_path = "../../../../../../../../c/build/win32/output/dxl_x86_c.dll"; + const string dll_path = "../../../../../../../c/build/win32/output/dxl_x86_c.dll"; #region PortHandler [DllImport(dll_path)] diff --git a/c#/protocol_combined/win32/protocol_combined/protocol_combined.csproj b/c#/protocol_combined/win32/protocol_combined/protocol_combined.csproj index 0015b985..76b105df 100644 --- a/c#/protocol_combined/win32/protocol_combined/protocol_combined.csproj +++ b/c#/protocol_combined/win32/protocol_combined/protocol_combined.csproj @@ -17,7 +17,7 @@ true full false - bin\Debug\ + bin\x86\Debug\ DEBUG;TRACE prompt 4 @@ -26,7 +26,7 @@ AnyCPU pdbonly true - bin\Release\ + bin\x86\Release\ TRACE prompt 4 diff --git a/c#/protocol_combined/win64/protocol_combined/dynamixel.cs b/c#/protocol_combined/win64/protocol_combined/dynamixel.cs index 2919d6c9..3c3cb890 100644 --- a/c#/protocol_combined/win64/protocol_combined/dynamixel.cs +++ b/c#/protocol_combined/win64/protocol_combined/dynamixel.cs @@ -23,7 +23,7 @@ namespace dynamixel_sdk { class dynamixel { - const string dll_path = "../../../../../../../../c/build/win64/output/dxl_x64_c.dll"; + const string dll_path = "../../../../../../../c/build/win64/output/dxl_x64_c.dll"; #region PortHandler [DllImport(dll_path)] @@ -46,18 +46,13 @@ class dynamixel [DllImport(dll_path)] public static extern int getBaudRate (int port_num); - #ifdef __linux__ [DllImport(dll_path)] - public static extern int getBytesAvailable (int port_num); - #endif - - [DllImport(dll_path)] - public static extern int readPort (int port_num, uint8_t *packet, int length); + public static extern int readPort (int port_num, byte[] packet, int length); [DllImport(dll_path)] - public static extern int writePort (int port_num, uint8_t *packet, int length); + public static extern int writePort (int port_num, byte[] packet, int length); [DllImport(dll_path)] - public static extern void setPacketTimeout (int port_num, uint16_t packet_length); + public static extern void setPacketTimeout (int port_num, UInt16 packet_length); [DllImport(dll_path)] public static extern void setPacketTimeoutMSec(int port_num, double msec); [DllImport(dll_path)] @@ -68,12 +63,11 @@ class dynamixel [DllImport(dll_path)] public static extern void packetHandler (); - [DllImport(dll_path)] public static extern IntPtr getTxRxResult (int protocol_version, int result); [DllImport(dll_path)] public static extern IntPtr getRxPacketError (int protocol_version, byte error); - + [DllImport(dll_path)] public static extern int getLastTxRxResult (int port_num, int protocol_version); [DllImport(dll_path)] diff --git a/c++/example/protocol2.0/clear_multi_turn/clear_multi_turn.cpp b/c++/example/protocol2.0/clear_multi_turn/clear_multi_turn.cpp new file mode 100644 index 00000000..e40ff594 --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/clear_multi_turn.cpp @@ -0,0 +1,314 @@ +/******************************************************************************* +* Copyright 2017 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +// +// ********* Clear Multi-Turn Example ********* +// +// +// Available Dynamixel model on this example : MX with Protocol 2.0 (firmware v42 or above), Dynamixel X-series (firmware v42 or above) +// This example is tested with a Dynamixel XM430-W350-R, and an U2D2 +// Be sure that Dynamixel properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +// + +#if defined(__linux__) || defined(__APPLE__) +#include +#include +#include +#define STDIN_FILENO 0 +#elif defined(_WIN32) || defined(_WIN64) +#include +#endif + +#include +#include + +#include "dynamixel_sdk.h" // Uses Dynamixel SDK library + +// Control table address +#define ADDR_OPERATING_MODE 11 // Control table address is different in Dynamixel model +#define ADDR_TORQUE_ENABLE 64 +#define ADDR_GOAL_POSITION 116 +#define ADDR_PRESENT_POSITION 132 + +// Protocol version +#define PROTOCOL_VERSION 2.0 // See which protocol version is used in the Dynamixel + +// Default setting +#define DXL_ID 1 // Dynamixel ID: 1 +#define BAUDRATE 57600 +#define DEVICENAME "/dev/ttyUSB0" // Check which port is being used on your controller + // ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +#define TORQUE_ENABLE 1 // Value for enabling the torque +#define TORQUE_DISABLE 0 // Value for disabling the torque +#define MAX_POSITION_VALUE 1048575 +#define DXL_MOVING_STATUS_THRESHOLD 20 // Dynamixel moving status threshold +#define EXT_POSITION_CONTROL_MODE 4 // Value for extended position control mode (operating mode) + +#define ESC_ASCII_VALUE 0x1b +#define SPACE_ASCII_VALUE 0x20 + +int getch() +{ +#if defined(__linux__) || defined(__APPLE__) + struct termios oldt, newt; + int ch; + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + ch = getchar(); + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + return ch; +#elif defined(_WIN32) || defined(_WIN64) + return _getch(); +#endif +} + +int kbhit(void) +{ +#if defined(__linux__) || defined(__APPLE__) + struct termios oldt, newt; + int ch; + int oldf; + + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + oldf = fcntl(STDIN_FILENO, F_GETFL, 0); + fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); + + ch = getchar(); + + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + fcntl(STDIN_FILENO, F_SETFL, oldf); + + if (ch != EOF) + { + ungetc(ch, stdin); + return 1; + } + + return 0; +#elif defined(_WIN32) || defined(_WIN64) + return _kbhit(); +#endif +} + +void msecSleep(int waitTime) +{ +#if defined(__linux__) || defined(__APPLE__) + usleep(waitTime * 1000); +#elif defined(_WIN32) || defined(_WIN64) + _sleep(waitTime); +#endif +} + +int main() +{ + // Initialize PortHandler instance + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME); + + // Initialize PacketHandler instance + // Set the protocol version + // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler + dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + uint8_t dxl_error = 0; // Dynamixel error + int32_t dxl_present_position = 0; // Present position + + // Open port + if (portHandler->openPort()) + { + printf("Succeeded to open the port!\n"); + } + else + { + printf("Failed to open the port!\n"); + printf("Press any key to terminate...\n"); + getch(); + return 0; + } + + // Set port baudrate + if (portHandler->setBaudRate(BAUDRATE)) + { + printf("Succeeded to change the baudrate!\n"); + } + else + { + printf("Failed to change the baudrate!\n"); + printf("Press any key to terminate...\n"); + getch(); + return 0; + } + + // Set operating mode to extended position control mode + dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) + { + printf("%s\n", packetHandler->getRxPacketError(dxl_error)); + } + else + { + printf("Operating mode changed to extended position control mode. \n"); + } + + + // Enable Dynamixel Torque + dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) + { + printf("%s\n", packetHandler->getRxPacketError(dxl_error)); + } + else + { + printf("Dynamixel has been successfully connected \n"); + } + + while(1) + { + printf("\nPress any key to continue! (or press ESC to quit!)\n"); + if (getch() == ESC_ASCII_VALUE) + break; + + printf(" Press SPACE key to clear multi-turn information! (or press ESC to stop!)\n"); + + // Write goal position + dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, MAX_POSITION_VALUE, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) + { + printf("%s\n", packetHandler->getRxPacketError(dxl_error)); + } + + do + { + // Read present position + dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION, (uint32_t*)&dxl_present_position, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) + { + printf("%s\n", packetHandler->getRxPacketError(dxl_error)); + } + + printf(" [ID:%03d] GoalPos:%03d PresPos:%03d\r", DXL_ID, MAX_POSITION_VALUE, dxl_present_position); + + if (kbhit()) + { + char c = getch(); + if (c == SPACE_ASCII_VALUE) + { + printf("\n Stop & Clear Multi-Turn Information! \n"); + + // Write the present position to the goal position to stop moving + dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, dxl_present_position, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) + { + printf("%s\n", packetHandler->getRxPacketError(dxl_error)); + } + + msecSleep(300); + + // Clear Multi-Turn Information + dxl_comm_result = packetHandler->clearMultiTurn(portHandler, DXL_ID, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) + { + printf("%s\n", packetHandler->getRxPacketError(dxl_error)); + } + + // Read present position + dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION, (uint32_t*)&dxl_present_position, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) + { + printf("%s\n", packetHandler->getRxPacketError(dxl_error)); + } + + printf(" Present Position has been reset. : %03d \n", dxl_present_position); + + break; + } + else if (c == ESC_ASCII_VALUE) + { + printf("\n Stopped!! \n"); + + // Write the present position to the goal position to stop moving + dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, dxl_present_position, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) + { + printf("%s\n", packetHandler->getRxPacketError(dxl_error)); + } + + break; + } + } + + }while((abs(MAX_POSITION_VALUE - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + printf("\n"); + } + + // Disable Dynamixel Torque + dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) + { + printf("%s\n", packetHandler->getRxPacketError(dxl_error)); + } + + // Close port + portHandler->closePort(); + + return 0; +} diff --git a/c++/example/protocol2.0/clear_multi_turn/linux32/Makefile b/c++/example/protocol2.0/clear_multi_turn/linux32/Makefile new file mode 100644 index 00000000..0d0e7906 --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/linux32/Makefile @@ -0,0 +1,80 @@ +################################################## +# PROJECT: DXL Protocol 2.0 Clear Multi-Turn Example Makefile +# AUTHOR : ROBOTIS Ltd. +################################################## + +#--------------------------------------------------------------------- +# Makefile template for projects using DXL SDK +# +# Please make sure to follow these instructions when setting up your +# own copy of this file: +# +# 1- Enter the name of the target (the TARGET variable) +# 2- Add additional source files to the SOURCES variable +# 3- Add additional static library objects to the OBJECTS variable +# if necessary +# 4- Ensure that compiler flags, INCLUDES, and LIBRARIES are +# appropriate to your needs +# +# +# This makefile will link against several libraries, not all of which +# are necessarily needed for your project. Please feel free to +# remove libaries you do not need. +#--------------------------------------------------------------------- + +# *** ENTER THE TARGET NAME HERE *** +TARGET = clear_multi_turn + +# important directories used by assorted rules and other variables +DIR_DXL = ../../../.. +DIR_OBJS = .objects + +# compiler options +CC = gcc +CX = g++ +CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g +CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g +LNKCC = $(CX) +LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib +FORMAT = -m32 + +#--------------------------------------------------------------------- +# Core components (all of these are likely going to be needed) +#--------------------------------------------------------------------- +INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk +LIBRARIES += -ldxl_x86_cpp +LIBRARIES += -lrt + +#--------------------------------------------------------------------- +# Files +#--------------------------------------------------------------------- +SOURCES = clear_multi_turn.cpp \ + # *** OTHER SOURCES GO HERE *** + +OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) +#OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** + + +#--------------------------------------------------------------------- +# Compiling Rules +#--------------------------------------------------------------------- +$(TARGET): make_directory $(OBJECTS) + $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) + +all: $(TARGET) + +clean: + rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo + +make_directory: + mkdir -p $(DIR_OBJS)/ + +$(DIR_OBJS)/%.o: ../%.c + $(CC) $(CCFLAGS) -c $? -o $@ + +$(DIR_OBJS)/%.o: ../%.cpp + $(CX) $(CXFLAGS) -c $? -o $@ + +#--------------------------------------------------------------------- +# End of Makefile +#--------------------------------------------------------------------- diff --git a/c++/example/protocol2.0/clear_multi_turn/linux64/Makefile b/c++/example/protocol2.0/clear_multi_turn/linux64/Makefile new file mode 100644 index 00000000..1059c159 --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/linux64/Makefile @@ -0,0 +1,80 @@ +################################################## +# PROJECT: DXL Protocol 2.0 Clear Multi-Turn Example Makefile +# AUTHOR : ROBOTIS Ltd. +################################################## + +#--------------------------------------------------------------------- +# Makefile template for projects using DXL SDK +# +# Please make sure to follow these instructions when setting up your +# own copy of this file: +# +# 1- Enter the name of the target (the TARGET variable) +# 2- Add additional source files to the SOURCES variable +# 3- Add additional static library objects to the OBJECTS variable +# if necessary +# 4- Ensure that compiler flags, INCLUDES, and LIBRARIES are +# appropriate to your needs +# +# +# This makefile will link against several libraries, not all of which +# are necessarily needed for your project. Please feel free to +# remove libaries you do not need. +#--------------------------------------------------------------------- + +# *** ENTER THE TARGET NAME HERE *** +TARGET = clear_multi_turn + +# important directories used by assorted rules and other variables +DIR_DXL = ../../../.. +DIR_OBJS = .objects + +# compiler options +CC = gcc +CX = g++ +CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g +CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g +LNKCC = $(CX) +LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib +FORMAT = -m64 + +#--------------------------------------------------------------------- +# Core components (all of these are likely going to be needed) +#--------------------------------------------------------------------- +INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk +LIBRARIES += -ldxl_x64_cpp +LIBRARIES += -lrt + +#--------------------------------------------------------------------- +# Files +#--------------------------------------------------------------------- +SOURCES = clear_multi_turn.cpp \ + # *** OTHER SOURCES GO HERE *** + +OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) +#OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** + + +#--------------------------------------------------------------------- +# Compiling Rules +#--------------------------------------------------------------------- +$(TARGET): make_directory $(OBJECTS) + $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) + +all: $(TARGET) + +clean: + rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo + +make_directory: + mkdir -p $(DIR_OBJS)/ + +$(DIR_OBJS)/%.o: ../%.c + $(CC) $(CCFLAGS) -c $? -o $@ + +$(DIR_OBJS)/%.o: ../%.cpp + $(CX) $(CXFLAGS) -c $? -o $@ + +#--------------------------------------------------------------------- +# End of Makefile +#--------------------------------------------------------------------- diff --git a/c++/example/protocol2.0/clear_multi_turn/linux_sbc/Makefile b/c++/example/protocol2.0/clear_multi_turn/linux_sbc/Makefile new file mode 100644 index 00000000..5f5758e1 --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/linux_sbc/Makefile @@ -0,0 +1,79 @@ +################################################## +# PROJECT: DXL Protocol 2.0 Clear Multi-Turn Example Makefile +# AUTHOR : ROBOTIS Ltd. +################################################## + +#--------------------------------------------------------------------- +# Makefile template for projects using DXL SDK +# +# Please make sure to follow these instructions when setting up your +# own copy of this file: +# +# 1- Enter the name of the target (the TARGET variable) +# 2- Add additional source files to the SOURCES variable +# 3- Add additional static library objects to the OBJECTS variable +# if necessary +# 4- Ensure that compiler flags, INCLUDES, and LIBRARIES are +# appropriate to your needs +# +# +# This makefile will link against several libraries, not all of which +# are necessarily needed for your project. Please feel free to +# remove libaries you do not need. +#--------------------------------------------------------------------- + +# *** ENTER THE TARGET NAME HERE *** +TARGET = clear_multi_turn + +# important directories used by assorted rules and other variables +DIR_DXL = ../../../.. +DIR_OBJS = .objects + +# compiler options +CC = gcc +CX = g++ +CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) -g +CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) -g +LNKCC = $(CX) +LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib + +#--------------------------------------------------------------------- +# Core components (all of these are likely going to be needed) +#--------------------------------------------------------------------- +INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk +LIBRARIES += -ldxl_sbc_cpp +LIBRARIES += -lrt + +#--------------------------------------------------------------------- +# Files +#--------------------------------------------------------------------- +SOURCES = clear_multi_turn.cpp \ + # *** OTHER SOURCES GO HERE *** + +OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) +#OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** + + +#--------------------------------------------------------------------- +# Compiling Rules +#--------------------------------------------------------------------- +$(TARGET): make_directory $(OBJECTS) + $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) + +all: $(TARGET) + +clean: + rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo + +make_directory: + mkdir -p $(DIR_OBJS)/ + +$(DIR_OBJS)/%.o: ../%.c + $(CC) $(CCFLAGS) -c $? -o $@ + +$(DIR_OBJS)/%.o: ../%.cpp + $(CX) $(CXFLAGS) -c $? -o $@ + +#--------------------------------------------------------------------- +# End of Makefile +#--------------------------------------------------------------------- diff --git a/c++/example/protocol2.0/clear_multi_turn/mac/Makefile b/c++/example/protocol2.0/clear_multi_turn/mac/Makefile new file mode 100644 index 00000000..f2d8206d --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/mac/Makefile @@ -0,0 +1,78 @@ +################################################## +# PROJECT: DXL Protocol 2.0 Clear Multi-Turn Example Makefile +# AUTHOR : ROBOTIS Ltd. +################################################## + +#--------------------------------------------------------------------- +# Makefile template for projects using DXL SDK +# +# Please make sure to follow these instructions when setting up your +# own copy of this file: +# +# 1- Enter the name of the target (the TARGET variable) +# 2- Add additional source files to the SOURCES variable +# 3- Add additional static library objects to the OBJECTS variable +# if necessary +# 4- Ensure that compiler flags, INCLUDES, and LIBRARIES are +# appropriate to your needs +# +# +# This makefile will link against several libraries, not all of which +# are necessarily needed for your project. Please feel free to +# remove libaries you do not need. +#--------------------------------------------------------------------- + +# *** ENTER THE TARGET NAME HERE *** +TARGET = clear_multi_turn + +# important directories used by assorted rules and other variables +DIR_DXL = ../../../.. +DIR_OBJS = .objects + +# compiler options +CC = gcc +CX = g++ +CCFLAGS = -O2 -O3 -D_GNU_SOURCE -Wall $(INCLUDES) -g +CXFLAGS = -O2 -O3 -D_GNU_SOURCE -Wall $(INCLUDES) -g +LNKCC = $(CX) +LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib + +#--------------------------------------------------------------------- +# Core components (all of these are likely going to be needed) +#--------------------------------------------------------------------- +INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk +LIBRARIES += -ldxl_mac_cpp + +#--------------------------------------------------------------------- +# Files +#--------------------------------------------------------------------- +SOURCES = clear_multi_turn.cpp \ + # *** OTHER SOURCES GO HERE *** + +OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) +#OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** + + +#--------------------------------------------------------------------- +# Compiling Rules +#--------------------------------------------------------------------- +$(TARGET): make_directory $(OBJECTS) + $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) + +all: $(TARGET) + +clean: + rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo + +make_directory: + mkdir -p $(DIR_OBJS)/ + +$(DIR_OBJS)/%.o: ../%.c + $(CC) $(CCFLAGS) -c $? -o $@ + +$(DIR_OBJS)/%.o: ../%.cpp + $(CX) $(CXFLAGS) -c $? -o $@ + +#--------------------------------------------------------------------- +# End of Makefile +#--------------------------------------------------------------------- diff --git a/c++/example/protocol2.0/clear_multi_turn/win32/.vs/clear_multi_turn/v14/.suo b/c++/example/protocol2.0/clear_multi_turn/win32/.vs/clear_multi_turn/v14/.suo new file mode 100644 index 00000000..a3f42d99 Binary files /dev/null and b/c++/example/protocol2.0/clear_multi_turn/win32/.vs/clear_multi_turn/v14/.suo differ diff --git a/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn.sln b/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn.sln new file mode 100644 index 00000000..4e8b2195 --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "clear_multi_turn", "clear_multi_turn\clear_multi_turn.vcxproj", "{A0FAEFF9-A4BD-4838-871B-B914EF211650}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Debug|x64.ActiveCfg = Debug|x64 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Debug|x64.Build.0 = Debug|x64 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Debug|x86.ActiveCfg = Debug|Win32 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Debug|x86.Build.0 = Debug|Win32 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Release|x64.ActiveCfg = Release|x64 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Release|x64.Build.0 = Release|x64 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Release|x86.ActiveCfg = Release|Win32 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj b/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj new file mode 100644 index 00000000..7eeb57d5 --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj @@ -0,0 +1,154 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + {A0FAEFF9-A4BD-4838-871B-B914EF211650} + Win32Proj + clear_multi_turn + 8.1 + + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + $(VC_LibraryPath_x86);$(WindowsSDK_LibraryPath_x86);$(NETFXKitsDir)Lib\um\x86;..\..\..\..\..\build\win32\output + + + false + + + + + + Level3 + Disabled + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + + + + + + + Level3 + Disabled + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + + + + + Level3 + + + MaxSpeed + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\..\..\..\include\dynamixel_sdk + CompileAsCpp + + + Console + true + true + true + dxl_x86_cpp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + Level3 + + + MaxSpeed + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + true + true + + + + + + + + + diff --git a/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.filters b/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.filters new file mode 100644 index 00000000..21141bfb --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + + + Source Files + + + diff --git a/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.user b/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.user new file mode 100644 index 00000000..45bf49fb --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.user @@ -0,0 +1,7 @@ + + + + PATH=%PATH%;..\..\..\..\..\build\win32\output; + WindowsLocalDebugger + + \ No newline at end of file diff --git a/c++/example/protocol2.0/clear_multi_turn/win64/.vs/clear_multi_turn/v14/.suo b/c++/example/protocol2.0/clear_multi_turn/win64/.vs/clear_multi_turn/v14/.suo new file mode 100644 index 00000000..08a021c2 Binary files /dev/null and b/c++/example/protocol2.0/clear_multi_turn/win64/.vs/clear_multi_turn/v14/.suo differ diff --git a/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn.sln b/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn.sln new file mode 100644 index 00000000..4e8b2195 --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "clear_multi_turn", "clear_multi_turn\clear_multi_turn.vcxproj", "{A0FAEFF9-A4BD-4838-871B-B914EF211650}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Debug|x64.ActiveCfg = Debug|x64 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Debug|x64.Build.0 = Debug|x64 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Debug|x86.ActiveCfg = Debug|Win32 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Debug|x86.Build.0 = Debug|Win32 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Release|x64.ActiveCfg = Release|x64 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Release|x64.Build.0 = Release|x64 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Release|x86.ActiveCfg = Release|Win32 + {A0FAEFF9-A4BD-4838-871B-B914EF211650}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj b/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj new file mode 100644 index 00000000..43585907 --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj @@ -0,0 +1,156 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + {A0FAEFF9-A4BD-4838-871B-B914EF211650} + Win32Proj + clear_multi_turn + 8.1 + + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + + + false + $(SolutionDir)$(Configuration)\ + $(Configuration)\ + $(VC_LibraryPath_x64);$(WindowsSDK_LibraryPath_x64);$(NETFXKitsDir)Lib\um\x64;..\..\..\..\..\build\win64\output + + + + + + Level3 + Disabled + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + + + + + + + Level3 + Disabled + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + + + + + Level3 + + + MaxSpeed + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + true + true + + + + + Level3 + + + MaxSpeed + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\..\..\..\include\dynamixel_sdk + CompileAsCpp + + + Console + true + true + true + dxl_x64_cpp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + + + + + diff --git a/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.filters b/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.filters new file mode 100644 index 00000000..21141bfb --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + + + Source Files + + + diff --git a/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.user b/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.user new file mode 100644 index 00000000..0f71dd30 --- /dev/null +++ b/c++/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.user @@ -0,0 +1,7 @@ + + + + PATH=%PATH%;..\..\..\..\..\build\win64\output; + WindowsLocalDebugger + + \ No newline at end of file diff --git a/c++/include/dynamixel_sdk/packet_handler.h b/c++/include/dynamixel_sdk/packet_handler.h index 37ff2400..8b20e26f 100644 --- a/c++/include/dynamixel_sdk/packet_handler.h +++ b/c++/include/dynamixel_sdk/packet_handler.h @@ -24,6 +24,11 @@ #if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include + +#define ERROR_PRINT SerialBT2.print +#else +#define ERROR_PRINT printf + #endif #include @@ -52,6 +57,7 @@ #define INST_BULK_READ 146 // 0x92 // --- Only for 2.0 --- // #define INST_REBOOT 8 +#define INST_CLEAR 16 // 0x10 #define INST_STATUS 85 // 0x55 #define INST_SYNC_READ 130 // 0x82 #define INST_BULK_WRITE 147 // 0x93 @@ -222,6 +228,19 @@ class WINDECLSPEC PacketHandler //////////////////////////////////////////////////////////////////////////////// virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reset multi-turn revolution information of Dynamixel + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), + /// @description Dynamixel X-series (Firmware v42 or above). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + //////////////////////////////////////////////////////////////////////////////// /// @brief The function that makes Dynamixel reset as it was produced in the factory /// @description The function makes an instruction packet with INST_FACTORY_RESET, @@ -491,7 +510,7 @@ class WINDECLSPEC PacketHandler /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits the packet with PacketHandler::txPacket(). - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write @@ -506,7 +525,7 @@ class WINDECLSPEC PacketHandler /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits and receives the packet with PacketHandler::txRxPacket(), /// @description gets the error from the packet. - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write diff --git a/c++/include/dynamixel_sdk/protocol1_packet_handler.h b/c++/include/dynamixel_sdk/protocol1_packet_handler.h index ebfa05c0..3dcbadd1 100644 --- a/c++/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/c++/include/dynamixel_sdk/protocol1_packet_handler.h @@ -178,6 +178,15 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that reset multi-turn revolution information of Dynamixel + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// /// @brief The function that makes Dynamixel reset as it was produced in the factory /// @description The function makes an instruction packet with INST_FACTORY_RESET, @@ -447,7 +456,7 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits the packet with Protocol1PacketHandler::txPacket(). - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write @@ -462,7 +471,7 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits and receives the packet with Protocol1PacketHandler::txRxPacket(), /// @description gets the error from the packet. - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write diff --git a/c++/include/dynamixel_sdk/protocol2_packet_handler.h b/c++/include/dynamixel_sdk/protocol2_packet_handler.h index 3c953f39..b8bd8856 100644 --- a/c++/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/c++/include/dynamixel_sdk/protocol2_packet_handler.h @@ -186,6 +186,19 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reset multi-turn revolution information of Dynamixel + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). + /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), + /// @description Dynamixel X-series (Firmware v42 or above). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// /// @brief The function that makes Dynamixel reset as it was produced in the factory /// @description The function makes an instruction packet with INST_FACTORY_RESET, @@ -455,7 +468,7 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits the packet with Protocol2PacketHandler::txPacket(). - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write @@ -470,7 +483,7 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits and receives the packet with Protocol2PacketHandler::txRxPacket(), /// @description gets the error from the packet. - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write diff --git a/c++/src/dynamixel_sdk/group_bulk_read.cpp b/c++/src/dynamixel_sdk/group_bulk_read.cpp index bcea7551..edd1ad69 100644 --- a/c++/src/dynamixel_sdk/group_bulk_read.cpp +++ b/c++/src/dynamixel_sdk/group_bulk_read.cpp @@ -232,14 +232,5 @@ bool GroupBulkRead::getError(uint8_t id, uint8_t* error) // TODO : check protocol version, last_result_, data_list // if (last_result_ == false || error_list_.find(id) == error_list_.end()) - error[0] = error_list_[id][0]; - - if (error[0] != 0) - { - return true; - } - else - { - return false; - } + return error[0] = error_list_[id][0]; } \ No newline at end of file diff --git a/c++/src/dynamixel_sdk/group_sync_read.cpp b/c++/src/dynamixel_sdk/group_sync_read.cpp index f392943b..79e5f3b8 100644 --- a/c++/src/dynamixel_sdk/group_sync_read.cpp +++ b/c++/src/dynamixel_sdk/group_sync_read.cpp @@ -201,14 +201,5 @@ bool GroupSyncRead::getError(uint8_t id, uint8_t* error) // TODO : check protocol version, last_result_, data_list // if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || error_list_.find(id) == error_list_.end()) - error[0] = error_list_[id][0]; - - if (error[0] != 0) - { - return true; - } - else - { - return false; - } + return error[0] = error_list_[id][0]; } \ No newline at end of file diff --git a/c++/src/dynamixel_sdk/port_handler_arduino.cpp b/c++/src/dynamixel_sdk/port_handler_arduino.cpp index 19ad599f..07add9ad 100644 --- a/c++/src/dynamixel_sdk/port_handler_arduino.cpp +++ b/c++/src/dynamixel_sdk/port_handler_arduino.cpp @@ -27,7 +27,7 @@ #define DYNAMIXEL_SERIAL Serial3 #endif -#define LATENCY_TIMER 16 // msec (USB latency timer) +#define LATENCY_TIMER 4 // msec (USB latency timer) using namespace dynamixel; @@ -91,10 +91,17 @@ void PortHandlerArduino::closePort() void PortHandlerArduino::clearPort() { + int temp __attribute__((unused)); #if defined(__OPENCR__) - DYNAMIXEL_SERIAL.flush(); + while (DYNAMIXEL_SERIAL.available()) + { + temp = DYNAMIXEL_SERIAL.read(); + } #elif defined(__OPENCM904__) - p_dxl_serial->flush(); + while (p_dxl_serial->available()) + { + temp = p_dxl_serial->read(); + } #endif } @@ -205,7 +212,7 @@ bool PortHandlerArduino::isPacketTimeout() double PortHandlerArduino::getCurrentTime() { - return (double)millis(); + return (double)millis(); } double PortHandlerArduino::getTimeSinceStart() @@ -285,8 +292,15 @@ void PortHandlerArduino::setTxEnable() void PortHandlerArduino::setTxDisable() { #if defined(__OPENCR__) +#ifdef SERIAL_WRITES_NON_BLOCKING + DYNAMIXEL_SERIAL.flush(); // make sure it completes before we disable... +#endif drv_dxl_tx_enable(FALSE); + #elif defined(__OPENCM904__) +#ifdef SERIAL_WRITES_NON_BLOCKING + p_dxl_serial->flush(); +#endif drv_dxl_tx_enable(socket_fd_, FALSE); #endif } diff --git a/c++/src/dynamixel_sdk/port_handler_linux.cpp b/c++/src/dynamixel_sdk/port_handler_linux.cpp index adf3ef82..9f563508 100644 --- a/c++/src/dynamixel_sdk/port_handler_linux.cpp +++ b/c++/src/dynamixel_sdk/port_handler_linux.cpp @@ -56,6 +56,27 @@ // or if you have another good idea that can be an alternatives, // please give us advice via github issue https://github.com/ROBOTIS-GIT/DynamixelSDK/issues +struct termios2 { + tcflag_t c_iflag; /* input mode flags */ + tcflag_t c_oflag; /* output mode flags */ + tcflag_t c_cflag; /* control mode flags */ + tcflag_t c_lflag; /* local mode flags */ + cc_t c_line; /* line discipline */ + cc_t c_cc[19]; /* control characters */ + speed_t c_ispeed; /* input speed */ + speed_t c_ospeed; /* output speed */ +}; + +#ifndef TCGETS2 +#define TCGETS2 _IOR('T', 0x2A, struct termios2) +#endif +#ifndef TCSETS2 +#define TCSETS2 _IOW('T', 0x2B, struct termios2) +#endif +#ifndef BOTHER +#define BOTHER 0010000 +#endif + using namespace dynamixel; PortHandlerLinux::PortHandlerLinux(const char *port_name) @@ -217,6 +238,19 @@ bool PortHandlerLinux::setupPort(int cflag_baud) bool PortHandlerLinux::setCustomBaudrate(int speed) { + struct termios2 options; + + if (ioctl(socket_fd_, TCGETS2, &options) != 01) + { + options.c_cflag &= ~CBAUD; + options.c_cflag |= BOTHER; + options.c_ispeed = speed; + options.c_ospeed = speed; + + if (ioctl(socket_fd_, TCSETS2, &options) != -1) + return true; + } + // try to set a custom divisor struct serial_struct ss; if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) diff --git a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp index f8efe111..668adfa3 100644 --- a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -365,6 +365,11 @@ int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error return COMM_NOT_AVAILABLE; } +int Protocol1PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error) +{ + return COMM_NOT_AVAILABLE; +} + int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) { uint8_t txpacket[6] = {0}; @@ -407,6 +412,9 @@ int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t lengt uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length+6); //uint8_t *rxpacket = new uint8_t[length+6]; + if (rxpacket == NULL) + return result; + do { result = rxPacket(port, rxpacket); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); @@ -436,8 +444,14 @@ int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add uint8_t txpacket[8] = {0}; uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); + if (rxpacket == NULL) + return result; + if (id >= BROADCAST_ID) + { + free(rxpacket); return COMM_NOT_AVAILABLE; + } txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = 4; @@ -534,6 +548,9 @@ int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t uint8_t *txpacket = (uint8_t *)malloc(length+7); //uint8_t *txpacket = new uint8_t[length+7]; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_WRITE; @@ -559,6 +576,9 @@ int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad //uint8_t *txpacket = new uint8_t[length+7]; uint8_t rxpacket[6] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_WRITE; @@ -615,6 +635,9 @@ int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 uint8_t *txpacket = (uint8_t *)malloc(length+6); //uint8_t *txpacket = new uint8_t[length+6]; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; @@ -640,6 +663,9 @@ int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t //uint8_t *txpacket = new uint8_t[length+6]; uint8_t rxpacket[6] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; @@ -669,6 +695,9 @@ int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM //uint8_t *txpacket = new uint8_t[param_length + 8]; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; @@ -694,6 +723,9 @@ int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM //uint8_t *txpacket = new uint8_t[param_length + 7]; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM txpacket[PKT_INSTRUCTION] = INST_BULK_READ; diff --git a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp index c897e620..06dae1d2 100644 --- a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -17,11 +17,14 @@ /* Author: zerom, Ryu Woon Jung (Leon) */ #if defined(__linux__) +#include #include "protocol2_packet_handler.h" #elif defined(__APPLE__) +#include #include "protocol2_packet_handler.h" #elif defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT +#include #include "protocol2_packet_handler.h" #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include "../../include/dynamixel_sdk/protocol2_packet_handler.h" @@ -31,8 +34,8 @@ #include #include -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) +#define TXPACKET_MAX_LEN (1*1024) +#define RXPACKET_MAX_LEN (1*1024) ///////////////// for Protocol 2.0 Packet ///////////////// #define PKT_HEADER0 0 @@ -140,7 +143,7 @@ const char *Protocol2PacketHandler::getRxPacketError(uint8_t error) unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) { uint16_t i; - uint16_t crc_table[256] = {0x0000, + static const uint16_t crc_table[256] = {0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, @@ -190,39 +193,48 @@ unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *da void Protocol2PacketHandler::addStuffing(uint8_t *packet) { - int i = 0, index = 0; int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); int packet_length_out = packet_length_in; - uint8_t temp[TXPACKET_MAX_LEN] = {0}; + + if (packet_length_in < 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD + return; - for (uint16_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) - temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC + uint8_t *packet_ptr; + uint16_t packet_length_before_crc = packet_length_in - 2; + for (uint16_t i = 3; i < packet_length_before_crc; i++) { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; + packet_ptr = &packet[i+PKT_INSTRUCTION-2]; + if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) packet_length_out++; + } + + if (packet_length_in == packet_length_out) // no stuffing required + return; + + uint16_t out_index = packet_length_out + 6 - 2; // last index before crc + uint16_t in_index = packet_length_in + 6 - 2; // last index before crc + while (out_index != in_index) + { + if (packet[in_index] == 0xFD && packet[in_index-1] == 0xFF && packet[in_index-2] == 0xFF) + { + packet[out_index--] = 0xFD; // byte stuffing + if (out_index != in_index) + { + packet[out_index--] = packet[in_index--]; // FD + packet[out_index--] = packet[in_index--]; // FF + packet[out_index--] = packet[in_index--]; // FF + } + } + else + { + packet[out_index--] = packet[in_index--]; } } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - - ////////////////////////// - if (packet_length_in != packet_length_out) - packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - /////////////////////////// - - for (uint16_t s = 0; s < index; s++) - packet[s] = temp[s]; - //memcpy(packet, temp, index); packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + + return; } void Protocol2PacketHandler::removeStuffing(uint8_t *packet) @@ -393,6 +405,11 @@ int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) break; } } +#if defined(__linux__) || defined(__APPLE__) + usleep(0); +#elif defined(_WIN32) || defined(_WIN64) + Sleep(0); +#endif } port->is_using_ = false; @@ -489,6 +506,8 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vectorgetBaudRate()) * 10.0; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = 3; txpacket[PKT_LENGTH_H] = 0; @@ -502,7 +521,8 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vectorsetPacketTimeout((uint16_t)(wait_length * 30)); + //port->setPacketTimeout((uint16_t)(wait_length * 30)); + port->setPacketTimeout(((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0); while(1) { @@ -595,6 +615,24 @@ int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error return txRxPacket(port, txpacket, rxpacket, error); } +int Protocol2PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error) +{ + uint8_t txpacket[15] = {0}; + uint8_t rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 8; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_CLEAR; + txpacket[PKT_PARAMETER0] = 0x01; + txpacket[PKT_PARAMETER0+1] = 0x44; + txpacket[PKT_PARAMETER0+2] = 0x58; + txpacket[PKT_PARAMETER0+3] = 0x4C; + txpacket[PKT_PARAMETER0+4] = 0x22; + + return txRxPacket(port, txpacket, rxpacket, error); +} + int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) { uint8_t txpacket[11] = {0}; @@ -641,8 +679,10 @@ int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t lengt int result = COMM_TX_FAIL; uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length + 11 + (length/3)); // (length/3): consider stuffing - //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing - + + if (rxpacket == NULL) + return result; + do { result = rxPacket(port, rxpacket); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); @@ -672,8 +712,14 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length + 11 + (length/3)); // (length/3): consider stuffing + if (rxpacket == NULL) + return result; + if (id >= BROADCAST_ID) + { + free(rxpacket); return COMM_NOT_AVAILABLE; + } txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = 7; @@ -770,8 +816,10 @@ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + + if (txpacket == NULL) + return result; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); @@ -796,10 +844,12 @@ int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -853,11 +903,13 @@ int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -879,12 +931,14 @@ int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -905,11 +959,14 @@ int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -933,12 +990,14 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); - //uint8_t *txpacket = new uint8_t[param_length + 14]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -961,12 +1020,14 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -992,12 +1053,14 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H diff --git a/c/build/win32/output/dxl_x86_c.dll b/c/build/win32/output/dxl_x86_c.dll index c765a651..95b8673d 100644 Binary files a/c/build/win32/output/dxl_x86_c.dll and b/c/build/win32/output/dxl_x86_c.dll differ diff --git a/c/build/win32/output/dxl_x86_c.lib b/c/build/win32/output/dxl_x86_c.lib index 44ea10ae..e26ba9dd 100644 Binary files a/c/build/win32/output/dxl_x86_c.lib and b/c/build/win32/output/dxl_x86_c.lib differ diff --git a/c/build/win64/output/dxl_x64_c.dll b/c/build/win64/output/dxl_x64_c.dll index 09ab0843..0a13ff90 100644 Binary files a/c/build/win64/output/dxl_x64_c.dll and b/c/build/win64/output/dxl_x64_c.dll differ diff --git a/c/build/win64/output/dxl_x64_c.lib b/c/build/win64/output/dxl_x64_c.lib index 3f350e2c..b14111ea 100644 Binary files a/c/build/win64/output/dxl_x64_c.lib and b/c/build/win64/output/dxl_x64_c.lib differ diff --git a/c/example/dxl_monitor/dxl_monitor.c b/c/example/dxl_monitor/dxl_monitor.c old mode 100644 new mode 100755 index d04db217..be01c3f6 --- a/c/example/dxl_monitor/dxl_monitor.c +++ b/c/example/dxl_monitor/dxl_monitor.c @@ -488,7 +488,7 @@ int main(int argc, char *argv[]) printf("Detected Dynamixel : \n"); for (id = 0; id < MAX_ID; id++) { - if (getBroadcastPingResult(port_num, id, PROTOCOL_VERSION2)) + if (getBroadcastPingResult(port_num, PROTOCOL_VERSION2, id)) printf("[ID:%03d]\n", id); } printf("\n"); diff --git a/c/example/protocol2.0/clear_multi_turn/clear_multi_turn.c b/c/example/protocol2.0/clear_multi_turn/clear_multi_turn.c new file mode 100644 index 00000000..5fce73b8 --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/clear_multi_turn.c @@ -0,0 +1,311 @@ +/******************************************************************************* +* Copyright 2017 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +// +// ********* Clear Multi-Turn Example ********* +// +// +// Available Dynamixel model on this example : MX with Protocol 2.0 (firmware v42 or above), Dynamixel X-series (firmware v42 or above) +// This example is designed for using a Dynamixel XM430-W350-R, and an U2D2. +// To use another Dynamixel model, such as MX series, see their details in E-Manual(emanual.robotis.com) and edit below "#define"d variables yourself. +// Be sure that Dynamixel properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +// + +#if defined(__linux__) || defined(__APPLE__) +#include +#include +#include +#define STDIN_FILENO 0 +#elif defined(_WIN32) || defined(_WIN64) +#include +#endif + +#include +#include +#include "dynamixel_sdk.h" // Uses Dynamixel SDK library + +// Control table address +#define ADDR_OPERATING_MODE 11 // Control table address is different in Dynamixel model +#define ADDR_TORQUE_ENABLE 64 +#define ADDR_GOAL_POSITION 116 +#define ADDR_PRESENT_POSITION 132 + +// Protocol version +#define PROTOCOL_VERSION 2.0 // See which protocol version is used in the Dynamixel + +// Default setting +#define DXL_ID 1 // Dynamixel ID: 1 +#define BAUDRATE 57600 +#define DEVICENAME "/dev/ttyUSB0" // Check which port is being used on your controller + // ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +#define TORQUE_ENABLE 1 // Value for enabling the torque +#define TORQUE_DISABLE 0 // Value for disabling the torque +#define MAX_POSITION_VALUE 1048575 +#define DXL_MOVING_STATUS_THRESHOLD 20 // Dynamixel moving status threshold +#define EXT_POSITION_CONTROL_MODE 4 // Value for extended position control mode (operating mode) + +#define ESC_ASCII_VALUE 0x1b +#define SPACE_ASCII_VALUE 0x20 + +int getch() +{ +#if defined(__linux__) || defined(__APPLE__) + struct termios oldt, newt; + int ch; + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + ch = getchar(); + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + return ch; +#elif defined(_WIN32) || defined(_WIN64) + return _getch(); +#endif +} + +int kbhit(void) +{ +#if defined(__linux__) || defined(__APPLE__) + struct termios oldt, newt; + int ch; + int oldf; + + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + oldf = fcntl(STDIN_FILENO, F_GETFL, 0); + fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); + + ch = getchar(); + + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + fcntl(STDIN_FILENO, F_SETFL, oldf); + + if (ch != EOF) + { + ungetc(ch, stdin); + return 1; + } + + return 0; +#elif defined(_WIN32) || defined(_WIN64) + return _kbhit(); +#endif +} + +void msecSleep(int waitTime) +{ +#if defined(__linux__) || defined(__APPLE__) + usleep(waitTime * 1000); +#elif defined(_WIN32) || defined(_WIN64) + _sleep(waitTime); +#endif +} + +int main() +{ + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + uint8_t dxl_error = 0; // Dynamixel error + int32_t dxl_present_position = 0; // Present position + + // Open port + if (openPort(port_num)) + { + printf("Succeeded to open the port!\n"); + } + else + { + printf("Failed to open the port!\n"); + printf("Press any key to terminate...\n"); + getch(); + return 0; + } + + // Set port baudrate + if (setBaudRate(port_num, BAUDRATE)) + { + printf("Succeeded to change the baudrate!\n"); + } + else + { + printf("Failed to change the baudrate!\n"); + printf("Press any key to terminate...\n"); + getch(); + return 0; + } + + // Set operating mode to extended position control mode + write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE); + if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + else + { + printf("Operating mode changed to extended position control mode. \n"); + } + + // Enable Dynamixel Torque + write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + else + { + printf("Dynamixel has been successfully connected \n"); + } + + while (1) + { + printf("\nPress any key to continue! (or press ESC to quit!)\n"); + if (getch() == ESC_ASCII_VALUE) + break; + + printf(" Press SPACE key to clear multi-turn information! (or press ESC to stop!)\n"); + + // Write goal position + write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, MAX_POSITION_VALUE); + if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + do + { + // Read present position + dxl_present_position = read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + printf(" [ID:%03d] GoalPos:%03d PresPos:%03d\r", DXL_ID, MAX_POSITION_VALUE, dxl_present_position); + + if (kbhit()) + { + char c = getch(); + if (c == SPACE_ASCII_VALUE) + { + printf("\n Stop & Clear Multi-Turn Information! \n"); + + // Write the present position to the goal position to stop moving + write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, dxl_present_position); + if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + msecSleep(300); + + // Clear Multi-Turn Information + clearMultiTurn(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + // Read present position + dxl_present_position = read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + printf(" Present Position has been reset. : %03d \n", dxl_present_position); + + break; + } + else if (c == ESC_ASCII_VALUE) + { + printf("\n Stopped!! \n"); + + // Write the present position to the goal position to stop moving + write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, dxl_present_position); + if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + break; + } + } + + } while ((abs(MAX_POSITION_VALUE - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + printf("\n"); + } + + // Disable Dynamixel Torque + write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + // Close port + closePort(port_num); + + return 0; +} diff --git a/c/example/protocol2.0/clear_multi_turn/linux32/Makefile b/c/example/protocol2.0/clear_multi_turn/linux32/Makefile new file mode 100644 index 00000000..ee02816b --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/linux32/Makefile @@ -0,0 +1,80 @@ +################################################## +# PROJECT: DXL Protocol 2.0 Clear Multi-Turn Example Makefile +# AUTHOR : ROBOTIS Ltd. +################################################## + +#--------------------------------------------------------------------- +# Makefile template for projects using DXL SDK +# +# Please make sure to follow these instructions when setting up your +# own copy of this file: +# +# 1- Enter the name of the target (the TARGET variable) +# 2- Add additional source files to the SOURCES variable +# 3- Add additional static library objects to the OBJECTS variable +# if necessary +# 4- Ensure that compiler flags, INCLUDES, and LIBRARIES are +# appropriate to your needs +# +# +# This makefile will link against several libraries, not all of which +# are necessarily needed for your project. Please feel free to +# remove libaries you do not need. +#--------------------------------------------------------------------- + +# *** ENTER THE TARGET NAME HERE *** +TARGET = clear_multi_turn + +# important directories used by assorted rules and other variables +DIR_DXL = ../../../.. +DIR_OBJS = .objects + +# compiler options +CC = gcc +CX = g++ +CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g +CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g +LNKCC = $(CX) +LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib +FORMAT = -m32 + +#--------------------------------------------------------------------- +# Core components (all of these are likely going to be needed) +#--------------------------------------------------------------------- +INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk +LIBRARIES += -ldxl_x86_c +LIBRARIES += -lrt + +#--------------------------------------------------------------------- +# Files +#--------------------------------------------------------------------- +SOURCES = clear_multi_turn.cpp \ + # *** OTHER SOURCES GO HERE *** + +OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) +#OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** + + +#--------------------------------------------------------------------- +# Compiling Rules +#--------------------------------------------------------------------- +$(TARGET): make_directory $(OBJECTS) + $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) + +all: $(TARGET) + +clean: + rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo + +make_directory: + mkdir -p $(DIR_OBJS)/ + +$(DIR_OBJS)/%.o: ../%.c + $(CC) $(CCFLAGS) -c $? -o $@ + +$(DIR_OBJS)/%.o: ../%.cpp + $(CX) $(CXFLAGS) -c $? -o $@ + +#--------------------------------------------------------------------- +# End of Makefile +#--------------------------------------------------------------------- diff --git a/c/example/protocol2.0/clear_multi_turn/linux64/Makefile b/c/example/protocol2.0/clear_multi_turn/linux64/Makefile new file mode 100644 index 00000000..d8845c85 --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/linux64/Makefile @@ -0,0 +1,80 @@ +################################################## +# PROJECT: DXL Protocol 2.0 Clear Multi-Turn Example Makefile +# AUTHOR : ROBOTIS Ltd. +################################################## + +#--------------------------------------------------------------------- +# Makefile template for projects using DXL SDK +# +# Please make sure to follow these instructions when setting up your +# own copy of this file: +# +# 1- Enter the name of the target (the TARGET variable) +# 2- Add additional source files to the SOURCES variable +# 3- Add additional static library objects to the OBJECTS variable +# if necessary +# 4- Ensure that compiler flags, INCLUDES, and LIBRARIES are +# appropriate to your needs +# +# +# This makefile will link against several libraries, not all of which +# are necessarily needed for your project. Please feel free to +# remove libaries you do not need. +#--------------------------------------------------------------------- + +# *** ENTER THE TARGET NAME HERE *** +TARGET = clear_multi_turn + +# important directories used by assorted rules and other variables +DIR_DXL = ../../../.. +DIR_OBJS = .objects + +# compiler options +CC = gcc +CX = g++ +CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g +CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g +LNKCC = $(CX) +LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib +FORMAT = -m64 + +#--------------------------------------------------------------------- +# Core components (all of these are likely going to be needed) +#--------------------------------------------------------------------- +INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk +LIBRARIES += -ldxl_x64_c +LIBRARIES += -lrt + +#--------------------------------------------------------------------- +# Files +#--------------------------------------------------------------------- +SOURCES = clear_multi_turn.cpp \ + # *** OTHER SOURCES GO HERE *** + +OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) +#OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** + + +#--------------------------------------------------------------------- +# Compiling Rules +#--------------------------------------------------------------------- +$(TARGET): make_directory $(OBJECTS) + $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) + +all: $(TARGET) + +clean: + rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo + +make_directory: + mkdir -p $(DIR_OBJS)/ + +$(DIR_OBJS)/%.o: ../%.c + $(CC) $(CCFLAGS) -c $? -o $@ + +$(DIR_OBJS)/%.o: ../%.cpp + $(CX) $(CXFLAGS) -c $? -o $@ + +#--------------------------------------------------------------------- +# End of Makefile +#--------------------------------------------------------------------- diff --git a/c/example/protocol2.0/clear_multi_turn/linux_sbc/Makefile b/c/example/protocol2.0/clear_multi_turn/linux_sbc/Makefile new file mode 100644 index 00000000..28874ba3 --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/linux_sbc/Makefile @@ -0,0 +1,79 @@ +################################################## +# PROJECT: DXL Protocol 2.0 Clear Multi-Turn Example Makefile +# AUTHOR : ROBOTIS Ltd. +################################################## + +#--------------------------------------------------------------------- +# Makefile template for projects using DXL SDK +# +# Please make sure to follow these instructions when setting up your +# own copy of this file: +# +# 1- Enter the name of the target (the TARGET variable) +# 2- Add additional source files to the SOURCES variable +# 3- Add additional static library objects to the OBJECTS variable +# if necessary +# 4- Ensure that compiler flags, INCLUDES, and LIBRARIES are +# appropriate to your needs +# +# +# This makefile will link against several libraries, not all of which +# are necessarily needed for your project. Please feel free to +# remove libaries you do not need. +#--------------------------------------------------------------------- + +# *** ENTER THE TARGET NAME HERE *** +TARGET = clear_multi_turn + +# important directories used by assorted rules and other variables +DIR_DXL = ../../../.. +DIR_OBJS = .objects + +# compiler options +CC = gcc +CX = g++ +CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) -g +CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) -g +LNKCC = $(CX) +LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib + +#--------------------------------------------------------------------- +# Core components (all of these are likely going to be needed) +#--------------------------------------------------------------------- +INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk +LIBRARIES += -ldxl_sbc_c +LIBRARIES += -lrt + +#--------------------------------------------------------------------- +# Files +#--------------------------------------------------------------------- +SOURCES = clear_multi_turn.cpp \ + # *** OTHER SOURCES GO HERE *** + +OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) +#OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** + + +#--------------------------------------------------------------------- +# Compiling Rules +#--------------------------------------------------------------------- +$(TARGET): make_directory $(OBJECTS) + $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) + +all: $(TARGET) + +clean: + rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo + +make_directory: + mkdir -p $(DIR_OBJS)/ + +$(DIR_OBJS)/%.o: ../%.c + $(CC) $(CCFLAGS) -c $? -o $@ + +$(DIR_OBJS)/%.o: ../%.cpp + $(CX) $(CXFLAGS) -c $? -o $@ + +#--------------------------------------------------------------------- +# End of Makefile +#--------------------------------------------------------------------- diff --git a/c/example/protocol2.0/clear_multi_turn/mac/Makefile b/c/example/protocol2.0/clear_multi_turn/mac/Makefile new file mode 100644 index 00000000..7bb24f8a --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/mac/Makefile @@ -0,0 +1,78 @@ +################################################## +# PROJECT: DXL Protocol 2.0 Clear Multi-Turn Example Makefile +# AUTHOR : ROBOTIS Ltd. +################################################## + +#--------------------------------------------------------------------- +# Makefile template for projects using DXL SDK +# +# Please make sure to follow these instructions when setting up your +# own copy of this file: +# +# 1- Enter the name of the target (the TARGET variable) +# 2- Add additional source files to the SOURCES variable +# 3- Add additional static library objects to the OBJECTS variable +# if necessary +# 4- Ensure that compiler flags, INCLUDES, and LIBRARIES are +# appropriate to your needs +# +# +# This makefile will link against several libraries, not all of which +# are necessarily needed for your project. Please feel free to +# remove libaries you do not need. +#--------------------------------------------------------------------- + +# *** ENTER THE TARGET NAME HERE *** +TARGET = clear_multi_turn + +# important directories used by assorted rules and other variables +DIR_DXL = ../../../.. +DIR_OBJS = .objects + +# compiler options +CC = gcc +CX = g++ +CCFLAGS = -O2 -O3 -D_GNU_SOURCE -Wall $(INCLUDES) -g +CXFLAGS = -O2 -O3 -D_GNU_SOURCE -Wall $(INCLUDES) -g +LNKCC = $(CX) +LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib + +#--------------------------------------------------------------------- +# Core components (all of these are likely going to be needed) +#--------------------------------------------------------------------- +INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk +LIBRARIES += -ldxl_mac_c + +#--------------------------------------------------------------------- +# Files +#--------------------------------------------------------------------- +SOURCES = clear_multi_turn.cpp \ + # *** OTHER SOURCES GO HERE *** + +OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) +#OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** + + +#--------------------------------------------------------------------- +# Compiling Rules +#--------------------------------------------------------------------- +$(TARGET): make_directory $(OBJECTS) + $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) + +all: $(TARGET) + +clean: + rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo + +make_directory: + mkdir -p $(DIR_OBJS)/ + +$(DIR_OBJS)/%.o: ../%.c + $(CC) $(CCFLAGS) -c $? -o $@ + +$(DIR_OBJS)/%.o: ../%.cpp + $(CX) $(CXFLAGS) -c $? -o $@ + +#--------------------------------------------------------------------- +# End of Makefile +#--------------------------------------------------------------------- diff --git a/c/example/protocol2.0/clear_multi_turn/win32/.vs/clear_multi_turn/v14/.suo b/c/example/protocol2.0/clear_multi_turn/win32/.vs/clear_multi_turn/v14/.suo new file mode 100644 index 00000000..4674a53e Binary files /dev/null and b/c/example/protocol2.0/clear_multi_turn/win32/.vs/clear_multi_turn/v14/.suo differ diff --git a/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn.sln b/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn.sln new file mode 100644 index 00000000..12e7d22e --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "clear_multi_turn", "clear_multi_turn\clear_multi_turn.vcxproj", "{4ECA1398-7E81-42F5-A613-9EAF0D178405}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {4ECA1398-7E81-42F5-A613-9EAF0D178405}.Debug|x64.ActiveCfg = Debug|x64 + {4ECA1398-7E81-42F5-A613-9EAF0D178405}.Debug|x64.Build.0 = Debug|x64 + {4ECA1398-7E81-42F5-A613-9EAF0D178405}.Debug|x86.ActiveCfg = Debug|Win32 + {4ECA1398-7E81-42F5-A613-9EAF0D178405}.Debug|x86.Build.0 = Debug|Win32 + {4ECA1398-7E81-42F5-A613-9EAF0D178405}.Release|x64.ActiveCfg = Release|x64 + {4ECA1398-7E81-42F5-A613-9EAF0D178405}.Release|x64.Build.0 = Release|x64 + {4ECA1398-7E81-42F5-A613-9EAF0D178405}.Release|x86.ActiveCfg = Release|Win32 + {4ECA1398-7E81-42F5-A613-9EAF0D178405}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj b/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj new file mode 100644 index 00000000..73d8838e --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj @@ -0,0 +1,154 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + {4ECA1398-7E81-42F5-A613-9EAF0D178405} + Win32Proj + clear_multi_turn + 8.1 + + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + $(VC_LibraryPath_x86);$(WindowsSDK_LibraryPath_x86);$(NETFXKitsDir)Lib\um\x86;..\..\..\..\..\build\win32\output + + + false + + + + + + Level3 + Disabled + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + + + + + + + Level3 + Disabled + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + + + + + Level3 + + + MaxSpeed + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\..\..\..\include\dynamixel_sdk + CompileAsC + + + Console + true + true + true + dxl_x86_c.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + Level3 + + + MaxSpeed + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + true + true + + + + + + + + + \ No newline at end of file diff --git a/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.filters b/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.filters new file mode 100644 index 00000000..30c571fe --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + + + Source Files + + + \ No newline at end of file diff --git a/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.user b/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.user new file mode 100644 index 00000000..45bf49fb --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/win32/clear_multi_turn/clear_multi_turn.vcxproj.user @@ -0,0 +1,7 @@ + + + + PATH=%PATH%;..\..\..\..\..\build\win32\output; + WindowsLocalDebugger + + \ No newline at end of file diff --git a/c/example/protocol2.0/clear_multi_turn/win64/.vs/clear_multi_turn/v14/.suo b/c/example/protocol2.0/clear_multi_turn/win64/.vs/clear_multi_turn/v14/.suo new file mode 100644 index 00000000..6e1118e2 Binary files /dev/null and b/c/example/protocol2.0/clear_multi_turn/win64/.vs/clear_multi_turn/v14/.suo differ diff --git a/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn.sln b/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn.sln new file mode 100644 index 00000000..ea15066a --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "clear_multi_turn", "clear_multi_turn\clear_multi_turn.vcxproj", "{4CD1BCED-7D97-4F75-9AAA-3027B620E030}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {4CD1BCED-7D97-4F75-9AAA-3027B620E030}.Debug|x64.ActiveCfg = Debug|x64 + {4CD1BCED-7D97-4F75-9AAA-3027B620E030}.Debug|x64.Build.0 = Debug|x64 + {4CD1BCED-7D97-4F75-9AAA-3027B620E030}.Debug|x86.ActiveCfg = Debug|Win32 + {4CD1BCED-7D97-4F75-9AAA-3027B620E030}.Debug|x86.Build.0 = Debug|Win32 + {4CD1BCED-7D97-4F75-9AAA-3027B620E030}.Release|x64.ActiveCfg = Release|x64 + {4CD1BCED-7D97-4F75-9AAA-3027B620E030}.Release|x64.Build.0 = Release|x64 + {4CD1BCED-7D97-4F75-9AAA-3027B620E030}.Release|x86.ActiveCfg = Release|Win32 + {4CD1BCED-7D97-4F75-9AAA-3027B620E030}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj b/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj new file mode 100644 index 00000000..02856b5f --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj @@ -0,0 +1,156 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + {4CD1BCED-7D97-4F75-9AAA-3027B620E030} + Win32Proj + clear_multi_turn + 8.1 + + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + Application + true + v140 + Unicode + + + Application + false + v140 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + + + false + $(SolutionDir)$(Configuration)\ + $(Configuration)\ + $(VC_LibraryPath_x64);$(WindowsSDK_LibraryPath_x64);$(NETFXKitsDir)Lib\um\x64;..\..\..\..\..\build\win64\output + + + + + + Level3 + Disabled + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + + + + + + + Level3 + Disabled + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + + + + + Level3 + + + MaxSpeed + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + + + Console + true + true + true + + + + + Level3 + + + MaxSpeed + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\..\..\..\include\dynamixel_sdk + CompileAsC + + + Console + true + true + true + dxl_x64_c.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.filters b/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.filters new file mode 100644 index 00000000..30c571fe --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + + + Source Files + + + \ No newline at end of file diff --git a/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.user b/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.user new file mode 100644 index 00000000..0f71dd30 --- /dev/null +++ b/c/example/protocol2.0/clear_multi_turn/win64/clear_multi_turn/clear_multi_turn.vcxproj.user @@ -0,0 +1,7 @@ + + + + PATH=%PATH%;..\..\..\..\..\build\win64\output; + WindowsLocalDebugger + + \ No newline at end of file diff --git a/c/include/dynamixel_sdk/packet_handler.h b/c/include/dynamixel_sdk/packet_handler.h index 416d69f9..66bb29e2 100644 --- a/c/include/dynamixel_sdk/packet_handler.h +++ b/c/include/dynamixel_sdk/packet_handler.h @@ -25,6 +25,11 @@ #include "robotis_def.h" #include "port_handler.h" +#if defined(__APPLE__) +#undef reboot +#define reboot rebootDXL +#endif + #define BROADCAST_ID 0xFE // 254 #define MAX_ID 0xFC // 252 @@ -47,6 +52,7 @@ #define INST_BULK_READ 146 // 0x92 // --- Only for 2.0 --- // #define INST_REBOOT 8 +#define INST_CLEAR 16 // 0x10 #define INST_STATUS 85 // 0x55 #define INST_SYNC_READ 130 // 0x82 #define INST_BULK_WRITE 147 // 0x93 @@ -100,7 +106,13 @@ WINDECLSPEC uint16_t pingGetModelNum (int port_num, int protocol_version, WINDECLSPEC void broadcastPing (int port_num, int protocol_version); WINDECLSPEC uint8_t getBroadcastPingResult (int port_num, int protocol_version, int id); +#if defined(__APPLE__) +WINDECLSPEC void rebootDXL (int port_num, int protocol_version, uint8_t id); +#else WINDECLSPEC void reboot (int port_num, int protocol_version, uint8_t id); +#endif + +WINDECLSPEC void clearMultiTurn (int port_num, int protocol_version, uint8_t id); WINDECLSPEC void factoryReset (int port_num, int protocol_version, uint8_t id, uint8_t option); diff --git a/c/include/dynamixel_sdk/protocol1_packet_handler.h b/c/include/dynamixel_sdk/protocol1_packet_handler.h index b9de05bc..bd0499fd 100644 --- a/c/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/c/include/dynamixel_sdk/protocol1_packet_handler.h @@ -44,6 +44,7 @@ WINDECLSPEC uint8_t getBroadcastPingResult1 (int port_num, int id); WINDECLSPEC void action1 (int port_num, uint8_t id); WINDECLSPEC void reboot1 (int port_num, uint8_t id); +WINDECLSPEC void clearMultiTurn1 (int port_num, uint8_t id); WINDECLSPEC void factoryReset1 (int port_num, uint8_t id, uint8_t option); WINDECLSPEC void readTx1 (int port_num, uint8_t id, uint16_t address, uint16_t length); diff --git a/c/include/dynamixel_sdk/protocol2_packet_handler.h b/c/include/dynamixel_sdk/protocol2_packet_handler.h index 6cacd8f0..1c188995 100644 --- a/c/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/c/include/dynamixel_sdk/protocol2_packet_handler.h @@ -48,6 +48,7 @@ WINDECLSPEC uint8_t getBroadcastPingResult2 (int port_num, int id); WINDECLSPEC void action2 (int port_num, uint8_t id); WINDECLSPEC void reboot2 (int port_num, uint8_t id); +WINDECLSPEC void clearMultiTurn2 (int port_num, uint8_t id); WINDECLSPEC void factoryReset2 (int port_num, uint8_t id, uint8_t option); WINDECLSPEC void readTx2 (int port_num, uint8_t id, uint16_t address, uint16_t length); diff --git a/c/src/dynamixel_sdk/group_bulk_read.c b/c/src/dynamixel_sdk/group_bulk_read.c index e5a0fcfd..900e0b94 100644 --- a/c/src/dynamixel_sdk/group_bulk_read.c +++ b/c/src/dynamixel_sdk/group_bulk_read.c @@ -183,6 +183,7 @@ void groupBulkReadRemoveParam(int group_num, uint8_t id) if (groupData[group_num].data_list[data_num].id == NOT_USED_ID) // NOT exist return; + free(groupData[group_num].data_list[data_num].data); groupData[group_num].data_list[data_num].data = 0; groupData[group_num].data_list[data_num].id = NOT_USED_ID; @@ -194,13 +195,22 @@ void groupBulkReadRemoveParam(int group_num, uint8_t id) void groupBulkReadClearParam(int group_num) { + int data_num = 0; int port_num = groupData[group_num].port_num; if (size(group_num) == 0) return; + for (data_num = 0; data_num < groupData[group_num].data_list_length; data_num++) + { + free(groupData[group_num].data_list[data_num].data); + groupData[group_num].data_list[data_num].data = 0; + } + + free(groupData[group_num].data_list); groupData[group_num].data_list = 0; + free(packetData[port_num].data_write); packetData[port_num].data_write = 0; groupData[group_num].data_list_length = 0; diff --git a/c/src/dynamixel_sdk/group_bulk_write.c b/c/src/dynamixel_sdk/group_bulk_write.c index 668128a3..d602cc85 100644 --- a/c/src/dynamixel_sdk/group_bulk_write.c +++ b/c/src/dynamixel_sdk/group_bulk_write.c @@ -219,6 +219,7 @@ void groupBulkWriteRemoveParam(int group_num, uint8_t id) groupData[group_num].data_list[data_num].data_end = 0; + free(groupData[group_num].data_list[data_num].data); groupData[group_num].data_list[data_num].data = 0; groupData[group_num].data_list[data_num].data_length = 0; @@ -274,6 +275,7 @@ uint8_t groupBulkWriteChangeParam(int group_num, uint8_t id, uint16_t start_addr } void groupBulkWriteClearParam(int group_num) { + int data_num = 0; int port_num = groupData[group_num].port_num; if (groupData[group_num].protocol_version == 1) @@ -282,8 +284,16 @@ void groupBulkWriteClearParam(int group_num) if (size(group_num) == 0) return; + for (data_num = 0; data_num < groupData[group_num].data_list_length; data_num++) + { + free(groupData[group_num].data_list[data_num].data); + groupData[group_num].data_list[data_num].data = 0; + } + + free(groupData[group_num].data_list); groupData[group_num].data_list = 0; + free(packetData[port_num].data_write); packetData[port_num].data_write = 0; groupData[group_num].data_list_length = 0; diff --git a/c/src/dynamixel_sdk/group_sync_read.c b/c/src/dynamixel_sdk/group_sync_read.c index 59932780..9b0dbc3c 100644 --- a/c/src/dynamixel_sdk/group_sync_read.c +++ b/c/src/dynamixel_sdk/group_sync_read.c @@ -173,6 +173,7 @@ void groupSyncReadRemoveParam(int group_num, uint8_t id) if (groupData[group_num].data_list[data_num].id == NOT_USED_ID) // NOT exist return; + free(groupData[group_num].data_list[data_num].data); groupData[group_num].data_list[data_num].data = 0; groupData[group_num].data_list[data_num].id = NOT_USED_ID; @@ -181,6 +182,7 @@ void groupSyncReadRemoveParam(int group_num, uint8_t id) } void groupSyncReadClearParam(int group_num) { + int data_num = 0; int port_num = groupData[group_num].port_num; if (groupData[group_num].protocol_version == 1) @@ -189,8 +191,16 @@ void groupSyncReadClearParam(int group_num) if (size(group_num) == 0) return; + for (data_num = 0; data_num < groupData[group_num].data_list_length; data_num++) + { + free(groupData[group_num].data_list[data_num].data); + groupData[group_num].data_list[data_num].data = 0; + } + + free(groupData[group_num].data_list); groupData[group_num].data_list = 0; + free(packetData[port_num].data_write); packetData[port_num].data_write = 0; groupData[group_num].data_list_length = 0; diff --git a/c/src/dynamixel_sdk/group_sync_write.c b/c/src/dynamixel_sdk/group_sync_write.c index 8469eb2a..df7cfa62 100644 --- a/c/src/dynamixel_sdk/group_sync_write.c +++ b/c/src/dynamixel_sdk/group_sync_write.c @@ -202,6 +202,7 @@ void groupSyncWriteRemoveParam(int group_num, uint8_t id) groupData[group_num].data_list[data_num].data_end = 0; + free(groupData[group_num].data_list[data_num].data); groupData[group_num].data_list[data_num].data = 0; groupData[group_num].data_list[data_num].id = NOT_USED_ID; @@ -251,13 +252,22 @@ uint8_t groupSyncWriteChangeParam(int group_num, uint8_t id, uint32_t data, uint void groupSyncWriteClearParam(int group_num) { + int data_num = 0; int port_num = groupData[group_num].port_num; if (size(group_num) == 0) return; + for (data_num = 0; data_num < groupData[group_num].data_list_length; data_num++) + { + free(groupData[group_num].data_list[data_num].data); + groupData[group_num].data_list[data_num].data = 0; + } + + free(groupData[group_num].data_list); groupData[group_num].data_list = 0; + free(packetData[port_num].data_write); packetData[port_num].data_write = 0; groupData[group_num].data_list_length = 0; diff --git a/c/src/dynamixel_sdk/packet_handler.c b/c/src/dynamixel_sdk/packet_handler.c index ae0cf3bb..fe4c0083 100644 --- a/c/src/dynamixel_sdk/packet_handler.c +++ b/c/src/dynamixel_sdk/packet_handler.c @@ -211,7 +211,11 @@ uint8_t getBroadcastPingResult(int port_num, int protocol_version, int id) } } +#if defined(__APPLE__) +void rebootDXL(int port_num, int protocol_version, uint8_t id) +#else void reboot(int port_num, int protocol_version, uint8_t id) +#endif { if (protocol_version == 1) { @@ -223,6 +227,18 @@ void reboot(int port_num, int protocol_version, uint8_t id) } } +void clearMultiTurn(int port_num, int protocol_version, uint8_t id) +{ + if (protocol_version == 1) + { + clearMultiTurn1(port_num, id); + } + else + { + clearMultiTurn2(port_num, id); + } +} + void factoryReset(int port_num, int protocol_version, uint8_t id, uint8_t option) { if (protocol_version == 1) diff --git a/c/src/dynamixel_sdk/port_handler_linux.c b/c/src/dynamixel_sdk/port_handler_linux.c index d506a4fa..965e64d1 100644 --- a/c/src/dynamixel_sdk/port_handler_linux.c +++ b/c/src/dynamixel_sdk/port_handler_linux.c @@ -56,6 +56,27 @@ // or if you have another good idea that can be an alternatives, // please give us advice via github issue https://github.com/ROBOTIS-GIT/DynamixelSDK/issues +struct termios2 { + tcflag_t c_iflag; /* input mode flags */ + tcflag_t c_oflag; /* output mode flags */ + tcflag_t c_cflag; /* control mode flags */ + tcflag_t c_lflag; /* local mode flags */ + cc_t c_line; /* line discipline */ + cc_t c_cc[19]; /* control characters */ + speed_t c_ispeed; /* input speed */ + speed_t c_ospeed; /* output speed */ +}; + +#ifndef TCGETS2 +#define TCGETS2 _IOR('T', 0x2A, struct termios2) +#endif +#ifndef TCSETS2 +#define TCSETS2 _IOW('T', 0x2B, struct termios2) +#endif +#ifndef BOTHER +#define BOTHER 0010000 +#endif + typedef struct { int socket_fd; @@ -263,6 +284,19 @@ uint8_t setupPortLinux(int port_num, int cflag_baud) uint8_t setCustomBaudrateLinux(int port_num, int speed) { + struct termios2 options; + + if (ioctl(portData[port_num].socket_fd, TCGETS2, &options) != 01) + { + options.c_cflag &= ~CBAUD; + options.c_cflag |= BOTHER; + options.c_ispeed = speed; + options.c_ospeed = speed; + + if (ioctl(portData[port_num].socket_fd, TCSETS2, &options) != -1) + return True; + } + // try to set a custom divisor struct serial_struct ss; if (ioctl(portData[port_num].socket_fd, TIOCGSERIAL, &ss) != 0) diff --git a/c/src/dynamixel_sdk/protocol1_packet_handler.c b/c/src/dynamixel_sdk/protocol1_packet_handler.c index 1e492a9c..5e1eee76 100644 --- a/c/src/dynamixel_sdk/protocol1_packet_handler.c +++ b/c/src/dynamixel_sdk/protocol1_packet_handler.c @@ -418,6 +418,11 @@ void reboot1(int port_num, uint8_t id) packetData[port_num].communication_result = COMM_NOT_AVAILABLE; } +void clearMultiTurn1(int port_num, uint8_t id) +{ + packetData[port_num].communication_result = COMM_NOT_AVAILABLE; +} + void factoryReset1(int port_num, uint8_t id, uint8_t option) { packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 6); diff --git a/c/src/dynamixel_sdk/protocol2_packet_handler.c b/c/src/dynamixel_sdk/protocol2_packet_handler.c index c0f475da..3f403482 100644 --- a/c/src/dynamixel_sdk/protocol2_packet_handler.c +++ b/c/src/dynamixel_sdk/protocol2_packet_handler.c @@ -17,11 +17,14 @@ /* Author: Ryu Woon Jung (Leon) */ #if defined(__linux__) +#include #include "protocol2_packet_handler.h" #elif defined(__APPLE__) +#include #include "protocol2_packet_handler.h" #elif defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT +#include #include "protocol2_packet_handler.h" #endif @@ -29,8 +32,8 @@ #include #include -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) +#define TXPACKET_MAX_LEN (1*1024) +#define RXPACKET_MAX_LEN (1*1024) ///////////////// for Protocol 2.0 Packet ///////////////// #define PKT_HEADER0 0 @@ -142,6 +145,11 @@ uint8_t getLastRxPacketError2(int port_num) void setDataWrite2(int port_num, uint16_t data_length, uint16_t data_pos, uint32_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, (data_pos + data_length) * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Set Data Write] memory allocation failed... \n"); + return; + } switch (data_length) { @@ -162,7 +170,7 @@ void setDataWrite2(int port_num, uint16_t data_length, uint16_t data_pos, uint32 break; default: - printf("[Set Data Write] failed... "); + printf("[Set Data Write] failed... \n"); break; } } @@ -181,7 +189,7 @@ uint32_t getDataRead2(int port_num, uint16_t data_length, uint16_t data_pos) , DXL_MAKEWORD(packetData[port_num].data_read[data_pos + 2], packetData[port_num].data_read[data_pos + 3])); default: - printf("[Set Data Read] failed... "); + printf("[Set Data Read] failed... \n"); return 0; } } @@ -189,7 +197,7 @@ uint32_t getDataRead2(int port_num, uint16_t data_length, uint16_t data_pos) unsigned short updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) { uint16_t i, j; - uint16_t crc_table[256] = { 0x0000, + static const uint16_t crc_table[256] = { 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, @@ -239,42 +247,52 @@ unsigned short updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t dat void addStuffing(uint8_t *packet) { - uint16_t s; - - uint16_t i = 0; - int index = 0; + uint8_t *packet_ptr; + uint16_t i; + uint16_t packet_length_before_crc; + uint16_t out_index, in_index; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); int packet_length_out = packet_length_in; - uint8_t temp[TXPACKET_MAX_LEN] = { 0 }; - - for (s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) - { - temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H - } - - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC + + if (packet_length_in < 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD + return; + + packet_length_before_crc = packet_length_in - 2; + for (i = 3; i < packet_length_before_crc; i++) { - temp[index++] = packet[i + PKT_INSTRUCTION]; - if (packet[i + PKT_INSTRUCTION] == 0xFD && packet[i + PKT_INSTRUCTION - 1] == 0xFF && packet[i + PKT_INSTRUCTION - 2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; + packet_ptr = &packet[i+PKT_INSTRUCTION-2]; + if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) packet_length_out++; - } } - temp[index++] = packet[PKT_INSTRUCTION + packet_length_in - 2]; - temp[index++] = packet[PKT_INSTRUCTION + packet_length_in - 1]; - - if (packet_length_in != packet_length_out) - packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - - for (s = 0; s < index; s++) + + if (packet_length_in == packet_length_out) // no stuffing required + return; + + out_index = packet_length_out + 6 - 2; // last index before crc + in_index = packet_length_in + 6 - 2; // last index before crc + while (out_index != in_index) { - packet[s] = temp[s]; + if (packet[in_index] == 0xFD && packet[in_index-1] == 0xFF && packet[in_index-2] == 0xFF) + { + packet[out_index--] = 0xFD; // byte stuffing + if (out_index != in_index) + { + packet[out_index--] = packet[in_index--]; // FD + packet[out_index--] = packet[in_index--]; // FF + packet[out_index--] = packet[in_index--]; // FF + } + } + else + { + packet[out_index--] = packet[in_index--]; + } } packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + + return; } void removeStuffing(uint8_t *packet) @@ -460,6 +478,11 @@ void rxPacket2(int port_num) break; } } +#if defined(__linux__) || defined(__APPLE__) + usleep(0); +#elif defined(_WIN32) || defined(_WIN64) + Sleep(0); +#endif } g_is_using[port_num] = False; @@ -520,7 +543,12 @@ uint16_t pingGetModelNum2(int port_num, uint8_t id) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 10); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 14); - + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[PingGetModeNum] memory allocation failed.. \n"); + return COMM_TX_FAIL; + } + if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; @@ -549,8 +577,14 @@ void broadcastPing2(int port_num) uint16_t rx_length = 0; uint16_t wait_length = STATUS_LENGTH * MAX_ID; - packetData[port_num].broadcast_ping_id_list = (uint8_t *)calloc(255, sizeof(uint8_t)); + double tx_time_per_byte = (1000.0 / (double)getBaudRate(port_num)) * 10.0; + packetData[port_num].broadcast_ping_id_list = (uint8_t *)calloc(255, sizeof(uint8_t)); + if (packetData[port_num].broadcast_ping_id_list == NULL) + { + printf("[BroadcastPing] memory allocation failed.. \n"); + return; + } for (id = 0; id < 255; id++) { @@ -559,6 +593,11 @@ void broadcastPing2(int port_num) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 10 * sizeof(uint8_t)); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, STATUS_LENGTH * MAX_ID * sizeof(uint8_t)); + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[BroadcastPing] memory allocation failed.. \n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = 3; @@ -573,7 +612,8 @@ void broadcastPing2(int port_num) } // set rx timeout - setPacketTimeout(port_num, (uint16_t)(wait_length * 30)); + //setPacketTimeout(port_num, (uint16_t)(wait_length * 30)); + setPacketTimeoutMSec(port_num, ((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0); while (1) { @@ -668,6 +708,11 @@ uint8_t getBroadcastPingResult2(int port_num, int id) void action2(int port_num, uint8_t id) { packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 10); + if (packetData[port_num].tx_packet == NULL) + { + printf("[Action] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = 3; @@ -681,6 +726,11 @@ void reboot2(int port_num, uint8_t id) { packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 10); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[Reboot] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = 3; @@ -690,11 +740,39 @@ void reboot2(int port_num, uint8_t id) txRxPacket2(port_num); } +void clearMultiTurn2(int port_num, uint8_t id) +{ + packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 15); + packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[ClearMultiTurn] memory allocation failed..\n"); + return; + } + + packetData[port_num].tx_packet[PKT_ID] = id; + packetData[port_num].tx_packet[PKT_LENGTH_L] = 8; + packetData[port_num].tx_packet[PKT_LENGTH_H] = 0; + packetData[port_num].tx_packet[PKT_INSTRUCTION] = INST_CLEAR; + packetData[port_num].tx_packet[PKT_PARAMETER0] = 0x01; + packetData[port_num].tx_packet[PKT_PARAMETER0+1] = 0x44; + packetData[port_num].tx_packet[PKT_PARAMETER0+2] = 0x58; + packetData[port_num].tx_packet[PKT_PARAMETER0+3] = 0x4C; + packetData[port_num].tx_packet[PKT_PARAMETER0+4] = 0x22; + + txRxPacket2(port_num); +} + void factoryReset2(int port_num, uint8_t id, uint8_t option) { packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 11); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); - + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[FactoryReset] memory allocation failed..\n"); + return; + } + packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = 4; packetData[port_num].tx_packet[PKT_LENGTH_H] = 0; @@ -709,7 +787,12 @@ void readTx2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].communication_result = COMM_TX_FAIL; packetData[port_num].tx_packet = (uint8_t *)malloc(14); - + if (packetData[port_num].tx_packet == NULL) + { + printf("[ReadTx] memory allocation failed..\n"); + return; + } + if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; @@ -727,6 +810,8 @@ void readTx2(int port_num, uint8_t id, uint16_t address, uint16_t length) txPacket2(port_num); + free(packetData[port_num].tx_packet); + // set packet timeout if (packetData[port_num].communication_result == COMM_SUCCESS) setPacketTimeout(port_num, (uint16_t)(length + 11)); @@ -738,7 +823,12 @@ void readRx2(int port_num, uint16_t length) packetData[port_num].communication_result = COMM_TX_FAIL; packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, RXPACKET_MAX_LEN); //(length + 11 + (length/3)); // (length/3): consider stuffing - + if (packetData[port_num].rx_packet == NULL) + { + printf("[ReadRx] memory allocation failed..\n"); + return; + } + rxPacket2(port_num); if (packetData[port_num].communication_result == COMM_SUCCESS) { @@ -759,7 +849,12 @@ void readTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 14); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, RXPACKET_MAX_LEN); //(length + 11 + (length/3)); // (length/3): consider stuffing - + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[ReadTxRx] memory allocation failed..\n"); + return; + } + if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; @@ -794,6 +889,11 @@ void read1ByteTx2(int port_num, uint8_t id, uint16_t address) uint8_t read1ByteRx2(int port_num) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 1 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read1ByteRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; readRx2(port_num, 1); if (packetData[port_num].communication_result == COMM_SUCCESS) @@ -803,6 +903,11 @@ uint8_t read1ByteRx2(int port_num) uint8_t read1ByteTxRx2(int port_num, uint8_t id, uint16_t address) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 1 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read1ByteTxRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; readTxRx2(port_num, id, address, 1); if (packetData[port_num].communication_result == COMM_SUCCESS) @@ -817,6 +922,11 @@ void read2ByteTx2(int port_num, uint8_t id, uint16_t address) uint16_t read2ByteRx2(int port_num) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 2 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read2ByteRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; packetData[port_num].data_read[1] = 0; readRx2(port_num, 2); @@ -827,6 +937,11 @@ uint16_t read2ByteRx2(int port_num) uint16_t read2ByteTxRx2(int port_num, uint8_t id, uint16_t address) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 2 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read2ByteTxRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; packetData[port_num].data_read[1] = 0; readTxRx2(port_num, id, address, 2); @@ -842,6 +957,11 @@ void read4ByteTx2(int port_num, uint8_t id, uint16_t address) uint32_t read4ByteRx2(int port_num) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 4 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read4ByteRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; packetData[port_num].data_read[1] = 0; packetData[port_num].data_read[2] = 0; @@ -854,6 +974,11 @@ uint32_t read4ByteRx2(int port_num) uint32_t read4ByteTxRx2(int port_num, uint8_t id, uint16_t address) { packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 4 * sizeof(uint8_t)); + if (packetData[port_num].data_read == NULL) + { + printf("[Read2ByteTxRx] memory allocation failed..\n"); + return 0; + } packetData[port_num].data_read[0] = 0; packetData[port_num].data_read[1] = 0; packetData[port_num].data_read[2] = 0; @@ -872,6 +997,11 @@ void writeTxOnly2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].communication_result = COMM_TX_FAIL; packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, length + 12); + if (packetData[port_num].tx_packet == NULL) + { + printf("[WriteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); @@ -897,7 +1027,12 @@ void writeTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, length + 12); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); - + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[WriteTxRx] memory allocation failed..\n"); + return; + } + packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); packetData[port_num].tx_packet[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); @@ -916,12 +1051,22 @@ void writeTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) void write1ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint8_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 1 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write1ByteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = data; writeTxOnly2(port_num, id, address, 1); } void write1ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint8_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 1 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write1ByteTxRx] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = data; writeTxRx2(port_num, id, address, 1); } @@ -929,6 +1074,11 @@ void write1ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint8_t data) void write2ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint16_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 2 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write2ByteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = DXL_LOBYTE(data); packetData[port_num].data_write[1] = DXL_HIBYTE(data); writeTxOnly2(port_num, id, address, 2); @@ -936,6 +1086,11 @@ void write2ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint16_t data void write2ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 2 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write2ByteTxRx] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = DXL_LOBYTE(data); packetData[port_num].data_write[1] = DXL_HIBYTE(data); writeTxRx2(port_num, id, address, 2); @@ -944,6 +1099,11 @@ void write2ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t data) void write4ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint32_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 4 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write4ByteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = DXL_LOBYTE(DXL_LOWORD(data)); packetData[port_num].data_write[1] = DXL_HIBYTE(DXL_LOWORD(data)); packetData[port_num].data_write[2] = DXL_LOBYTE(DXL_HIWORD(data)); @@ -953,6 +1113,11 @@ void write4ByteTxOnly2(int port_num, uint8_t id, uint16_t address, uint32_t data void write4ByteTxRx2(int port_num, uint8_t id, uint16_t address, uint32_t data) { packetData[port_num].data_write = (uint8_t *)realloc(packetData[port_num].data_write, 4 * sizeof(uint8_t)); + if (packetData[port_num].data_write == NULL) + { + printf("[Write4ByteTxRx] memory allocation failed..\n"); + return; + } packetData[port_num].data_write[0] = DXL_LOBYTE(DXL_LOWORD(data)); packetData[port_num].data_write[1] = DXL_HIBYTE(DXL_LOWORD(data)); packetData[port_num].data_write[2] = DXL_LOBYTE(DXL_HIWORD(data)); @@ -967,6 +1132,11 @@ void regWriteTxOnly2(int port_num, uint8_t id, uint16_t address, uint16_t length packetData[port_num].communication_result = COMM_TX_FAIL; packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, length + 12); + if (packetData[port_num].tx_packet == NULL) + { + printf("[RegWriteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); @@ -992,6 +1162,11 @@ void regWriteTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, length + 12); packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 11); + if (packetData[port_num].tx_packet == NULL || packetData[port_num].rx_packet == NULL) + { + printf("[RegWriteTxRx] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); @@ -1016,6 +1191,11 @@ void syncReadTx2(int port_num, uint16_t start_address, uint16_t data_length, uin packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, param_length + 14); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (packetData[port_num].tx_packet == NULL) + { + printf("[SyncReadTx] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1045,6 +1225,11 @@ void syncWriteTxOnly2(int port_num, uint16_t start_address, uint16_t data_length packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, param_length + 14); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (packetData[port_num].tx_packet == NULL) + { + printf("[SyncWriteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -1072,6 +1257,11 @@ void bulkReadTx2(int port_num, uint16_t param_length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, param_length + 10); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (packetData[port_num].tx_packet == NULL) + { + printf("[BulkReadTx] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -1103,6 +1293,11 @@ void bulkWriteTxOnly2(int port_num, uint16_t param_length) packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, param_length + 10); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (packetData[port_num].tx_packet == NULL) + { + printf("[BulkWriteTxOnly] memory allocation failed..\n"); + return; + } packetData[port_num].tx_packet[PKT_ID] = BROADCAST_ID; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H diff --git a/java/dynamixel_functions_java/x64/Dynamixel.java b/java/dynamixel_functions_java/x64/Dynamixel.java index c8e20f83..445fc7db 100644 --- a/java/dynamixel_functions_java/x64/Dynamixel.java +++ b/java/dynamixel_functions_java/x64/Dynamixel.java @@ -69,6 +69,8 @@ interface LibFunction extends Library public void reboot (int port_num, int protocol_version, byte id); + public void clearMultiTurn (int port_num, int protocol_version, byte id); + public void factoryReset (int port_num, int protocol_version, byte id, byte option); public void readTx (int port_num, int protocol_version, byte id, short address, short length); @@ -290,6 +292,11 @@ public void reboot(int port_num, int protocol_version, byte id) libFunction.reboot(port_num, protocol_version, id); } + public void clearMultiTurn(int port_num, int protocol_version, byte id) + { + libFunction.clearMultiTurn(port_num, protocol_version, id); + } + public void factoryReset(int port_num, int protocol_version, byte id, byte option) { libFunction.factoryReset(port_num, protocol_version, id, option); diff --git a/java/dynamixel_functions_java/x86/Dynamixel.java b/java/dynamixel_functions_java/x86/Dynamixel.java index bc5afbe6..494191a7 100644 --- a/java/dynamixel_functions_java/x86/Dynamixel.java +++ b/java/dynamixel_functions_java/x86/Dynamixel.java @@ -69,6 +69,8 @@ interface LibFunction extends Library public void reboot (int port_num, int protocol_version, byte id); + public void clearMultiTurn (int port_num, int protocol_version, byte id); + public void factoryReset (int port_num, int protocol_version, byte id, byte option); public void readTx (int port_num, int protocol_version, byte id, short address, short length); @@ -290,6 +292,11 @@ public void reboot(int port_num, int protocol_version, byte id) libFunction.reboot(port_num, protocol_version, id); } + public void clearMultiTurn(int port_num, int protocol_version, byte id) + { + libFunction.clearMultiTurn(port_num, protocol_version, id); + } + public void factoryReset(int port_num, int protocol_version, byte id, byte option) { libFunction.factoryReset(port_num, protocol_version, id, option); diff --git a/java/protocol2.0/clear_multi_turn/ClearMultiTurn.java b/java/protocol2.0/clear_multi_turn/ClearMultiTurn.java new file mode 100644 index 00000000..bb5548b3 --- /dev/null +++ b/java/protocol2.0/clear_multi_turn/ClearMultiTurn.java @@ -0,0 +1,261 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Author: Ki Jong Gil (Gilbert) */ + +// +// ********* Clear Multi-Turn Example ********* +// +// +// Available Dynamixel model on this example : MX with Protocol 2.0 (firmware v42 or above), Dynamixel X-series (firmware v42 or above) +// This example is designed for using a Dynamixel XM430-W350-R, and an U2D2. +// To use another Dynamixel model, such as MX series, see their details in E-Manual(emanual.robotis.com) and edit below "#define"d variables yourself. +// Be sure that Dynamixel properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +// + +import java.io.IOException; +import java.util.Scanner; + +public class ClearMultiTurn +{ + public static void main(String[] args) throws InterruptedException, IOException + { + // Control table address + short ADDR_OPERATING_MODE = 11; // Control table address is different in Dynamixel model + short ADDR_TORQUE_ENABLE = 64; + short ADDR_GOAL_POSITION = 116; + short ADDR_PRESENT_POSITION = 132; + + // Protocol version + int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + byte DXL_ID = 1; // Dynamixel ID: 1 + int BAUDRATE = 57600; + String DEVICENAME = "COM1"; // Check which port is being used on your controller + // ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + + byte TORQUE_ENABLE = 1; // Value for enabling the torque + byte TORQUE_DISABLE = 0; // Value for disabling the torque + int MAX_POSITION_VALUE = 1048575; + int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + byte EXT_POSITION_CONTROL_MODE = 4; // Value for extended position control mode (operating mode) + + + String KEY_FOR_ESCAPE = "e"; // Key for escape + String KEY_FOR_CONTINUE = " "; // key for continue + + int COMM_SUCCESS = 0; // Communication Success result value + int COMM_TX_FAIL = -1001; // Communication Tx Failed + + // Instead of getch + Scanner scanner = new Scanner(System.in); + + // Initialize Dynamixel class for java + Dynamixel dynamixel = new Dynamixel(); + + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + int dxl_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + System.out.println("Succeeded to open the port!"); + } + else + { + System.out.println("Failed to open the port!"); + System.out.println("Press any key to terminate..."); + scanner.nextLine(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + System.out.println("Succeeded to change the baudrate!"); + } + else + { + System.out.println("Failed to change the baudrate!"); + System.out.println("Press any key to terminate..."); + scanner.nextLine(); + return; + } + + // Set operating mode to extended position control mode + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + else + { + System.out.println("Operating mode changed to extended position control mode. \n"); + } + + // Enable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + else + { + System.out.println("Dynamixel has been successfully connected"); + } + + while (true) + { + System.out.println("\nPress enter to continue! (or press e then enter to quit!)"); + + String ch = scanner.nextLine(); + if (ch.equals("e")) + break; + + System.out.println("\nPress SPACE key to clear multi-turn information! (or press ESC to stop!)\n"); + + // Write goal position + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, MAX_POSITION_VALUE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + do + { + // Read present position + dxl_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + System.out.printf(" [ID: %d] GoalPos:%d PresPos:%d\n", DXL_ID, MAX_POSITION_VALUE, dxl_present_position); + + if (System.in.available() > 0) + { + + String c = scanner.nextLine(); + if(c.equals(KEY_FOR_CONTINUE)) + { + System.out.printf(" Stop & Clear Multi-Turn Information! \n"); + + // Write the present position to the goal position to stop moving + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, dxl_present_position); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + Thread.sleep(300); + + // Clear Multi-Turn Information + dynamixel.clearMultiTurn(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + // Read present position + dxl_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + System.out.printf(" Present Position has been reset. : %03d \n", dxl_present_position); + + break; + } + + else if (c.equals(KEY_FOR_ESCAPE)) + { + System.out.printf("\n Stopped!! "); + + // Write the present position to the goal position to stop moving + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, dxl_present_position); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + break; + } + } + + } while ((Math.abs(MAX_POSITION_VALUE - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + } + + // Disable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + System.out.println(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + System.out.println(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } +} \ No newline at end of file diff --git a/labview/protocol2.0/clear_multi_turn.vi b/labview/protocol2.0/clear_multi_turn.vi new file mode 100644 index 00000000..17d95fc2 Binary files /dev/null and b/labview/protocol2.0/clear_multi_turn.vi differ diff --git a/matlab/m_basic_function/packet_handler/clearMultiTurn.m b/matlab/m_basic_function/packet_handler/clearMultiTurn.m new file mode 100644 index 00000000..f7b63a8d --- /dev/null +++ b/matlab/m_basic_function/packet_handler/clearMultiTurn.m @@ -0,0 +1,36 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright 2018 ROBOTIS CO., LTD. +% +% Licensed under the Apache License, Version 2.0 (the "License"); +% you may not use this file except in compliance with the License. +% You may obtain a copy of the License at +% +% http://www.apache.org/licenses/LICENSE-2.0 +% +% Unless required by applicable law or agreed to in writing, software +% distributed under the License is distributed on an "AS IS" BASIS, +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +% See the License for the specific language governing permissions and +% limitations under the License. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Author: Ki Jong Gil (Gilbert) + +function [] = clearMultiTurn( port_num, protocol_version, id ) + +lib_name = ''; + +if strcmp(computer, 'PCWIN') + lib_name = 'dxl_x86_c'; +elseif strcmp(computer, 'PCWIN64') + lib_name = 'dxl_x64_c'; +elseif strcmp(computer, 'GLNX86') + lib_name = 'libdxl_x86_c'; +elseif strcmp(computer, 'GLNXA64') + lib_name = 'libdxl_x64_c'; +elseif strcmp(computer, 'MACI64') + lib_name = 'libdxl_mac_c'; +end + +calllib(lib_name, 'clearMultiTurn', port_num, protocol_version, id); +end diff --git a/matlab/protocol2.0/clear_multi_turn.m b/matlab/protocol2.0/clear_multi_turn.m new file mode 100644 index 00000000..3390a679 --- /dev/null +++ b/matlab/protocol2.0/clear_multi_turn.m @@ -0,0 +1,305 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Copyright 2017 ROBOTIS CO., LTD. +% +% Licensed under the Apache License, Version 2.0 (the "License"); +% you may not use this file except in compliance with the License. +% You may obtain a copy of the License at +% +% http://www.apache.org/licenses/LICENSE-2.0 +% +% Unless required by applicable law or agreed to in writing, software +% distributed under the License is distributed on an "AS IS" BASIS, +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +% See the License for the specific language governing permissions and +% limitations under the License. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Author: Ki Jong Gil (Gilbert) + +% +% ********* Clear Multi-Turn Example ********* +% +% +% Available Dynamixel model on this example : Dynamixel X-series (firmware v42 or above) +% This example is designed for using a Dynamixel XM430-W350-R, and an U2D2. +% To use another Dynamixel model, such as MX series, see their details in E-Manual(emanual.robotis.com) and edit below "#define"d variables yourself. +% Be sure that Dynamixel properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +% + +clc; +clear all; + +lib_name = ''; + +if strcmp(computer, 'PCWIN') + lib_name = 'dxl_x86_c'; +elseif strcmp(computer, 'PCWIN64') + lib_name = 'dxl_x64_c'; +elseif strcmp(computer, 'GLNX86') + lib_name = 'libdxl_x86_c'; +elseif strcmp(computer, 'GLNXA64') + lib_name = 'libdxl_x64_c'; +elseif strcmp(computer, 'MACI64') + lib_name = 'libdxl_mac_c'; +end + +% Load Libraries +if ~libisloaded(lib_name) + [notfound, warnings] = loadlibrary(lib_name, 'dynamixel_sdk.h', 'addheader', 'port_handler.h', 'addheader', 'packet_handler.h'); +end + +% Control table address +ADDR_OPERATING_MODE = 11; % Control table address is different in Dynamixel model +ADDR_TORQUE_ENABLE = 64; +ADDR_GOAL_POSITION = 116; +ADDR_PRESENT_POSITION = 132; + +% Protocol version +PROTOCOL_VERSION = 2.0; % See which protocol version is used in the Dynamixel + +% Default setting +DXL_ID = 1; % Dynamixel ID: 1 +BAUDRATE = 57600; +DEVICENAME = 'COM1'; % Check which port is being used on your controller + % ex) Windows: 'COM1' Linux: '/dev/ttyUSB0' Mac: '/dev/tty.usbserial-*' + +TORQUE_ENABLE = 1; % Value for enabling the torque +TORQUE_DISABLE = 0; % Value for disabling the torque +MAX_POSITION_VALUE = 1048575; +DXL_MOVING_STATUS_THRESHOLD = 20; % Dynamixel moving status threshold +EXT_POSITION_CONTROL_MODE = 4; % Value for extended position control mode (operating mode) + +ESC_CHARACTER = 'e'; % Key for escaping loop +SPACE_ASCII_VALUE = ' '; + +COMM_SUCCESS = 0; % Communication Success result value +COMM_TX_FAIL = -1001; % Communication Tx Failed + +% Initialize PortHandler Structs +% Set the port path +% Get methods and members of PortHandlerLinux or PortHandlerWindows +port_num = portHandler(DEVICENAME); + +% Initialize PacketHandler Structs +packetHandler(); + +dxl_comm_result = COMM_TX_FAIL; % Communication result + +dxl_error = 0; % Dynamixel error +dxl_present_position = 0; % Present position + +% Open port +if (openPort(port_num)) + fprintf('Succeeded to open the port!\n'); +else + unloadlibrary(lib_name); + fprintf('Failed to open the port!\n'); + input('Press any key to terminate...\n'); + return; +end + +% Set port baudrate +if (setBaudRate(port_num, BAUDRATE)) + fprintf('Succeeded to change the baudrate!\n'); +else + unloadlibrary(lib_name); + fprintf('Failed to change the baudrate!\n'); + input('Press any key to terminate...\n'); + return; +end + +% Set operating mode to extended position control mode +write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE); +dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION); +dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION); +if dxl_comm_result ~= COMM_SUCCESS + fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); +elseif dxl_error ~= 0 + fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error)); +else + fprintf('Operating mode changed to extended position control mode. \n'); +end + +% Enable Dynamixel Torque +write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE); +dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION); +dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION); +if dxl_comm_result ~= COMM_SUCCESS + fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); +elseif dxl_error ~= 0 + fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error)); +else + fprintf('Dynamixel has been successfully connected \n'); +end + +while 1 + if input('\nPress any key to continue! (or input e to quit!)', 's') == ESC_CHARACTER + break; + end + + fprintf(' Press SPACE key to clear multi-turn information! (or press e to stop!)'); + + % Write goal position + write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, MAX_POSITION_VALUE); + dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION); + dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION); + if dxl_comm_result ~= COMM_SUCCESS + fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + elseif dxl_error ~= 0 + fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error)); + end + + while 1 + ch = -1; + % Read present position + dxl_present_position = read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION); + dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION); + if dxl_comm_result ~= COMM_SUCCESS + fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + elseif dxl_error ~= 0 + fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error)); + end + + fprintf('\r [ID:%03d] GoalPos:%03d PresPos:%03d', DXL_ID, MAX_POSITION_VALUE, dxl_present_position); + ch = kbhit(0.2); + if ch == SPACE_ASCII_VALUE + fprintf('\n Stop & Clear Multi-Turn Information! \n'); + + % Write the present position to the goal position to stop moving + write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, typecast(uint32(dxl_present_position), 'int32')); + dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION); + dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION); + if dxl_comm_result ~= COMM_SUCCESS + fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + elseif dxl_error ~= 0 + fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error)); + end + + pause(0.5); + + % Clear Multi-Turn Information + clearMultiTurn(port_num, PROTOCOL_VERSION, DXL_ID); + dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION); + dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION); + if dxl_comm_result ~= COMM_SUCCESS + fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + elseif dxl_error ~= 0 + fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error)); + end + + % Read present position + dxl_present_position = read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRESENT_POSITION); + dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION); + dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION); + if dxl_comm_result ~= COMM_SUCCESS + fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + elseif dxl_error ~= 0 + fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error)); + end + + fprintf(' Present Position has been reset. : %03d \n', dxl_present_position); + break; + + elseif ch == ESC_CHARACTER + fprintf('\n Stopped!! \n'); + + % Write the present position to the goal position to stop moving + write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_GOAL_POSITION, typecast(uint32(dxl_present_position), 'int32')); + dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION); + dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION); + if dxl_comm_result ~= COMM_SUCCESS + fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); + elseif dxl_error ~= 0 + fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error)); + end + break; + end + + if ~(abs(MAX_POSITION_VALUE - typecast(uint32(dxl_present_position), 'int32')) > DXL_MOVING_STATUS_THRESHOLD) + break; + end + end +end + + +% Disable Dynamixel Torque +write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE); +dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION); +dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION); +if dxl_comm_result ~= COMM_SUCCESS + fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)); +elseif dxl_error ~= 0 + fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error)); +end + +% Close port +closePort(port_num); + +% Unload Library +unloadlibrary(lib_name); + +close all; + +function ch = kbhit(m) +% GETKEYWAIT - get a key within a time limit +% CH = GETKEYWAIT(P) waits for a keypress for a maximum of P seconds. P +% should be a positive number. CH is a double representing the key +% pressed key as an ascii number, including backspace (8), space (32), +% enter (13), etc. If a non-ascii key (Ctrl, Alt, etc.) is pressed, CH +% will be NaN. +% If no key is pressed within P seconds, -1 is returned, and if something +% went wrong during excution 0 is returned. +% +% See also INPUT, +% GETKEY (FileExchange) +% tested for Matlab 6.5 and higher +% version 2.1 (jan 2012) +% author : Jos van der Geest +% email : jos@jasen.nl +% History +% 1.0 (2005) creation +% 2.0 (apr 2009) - expanded error check on input argument, changed return +% values when a non-ascii was pressed (now NaN), or when something went +% wrong (now 0); added comments ; slight change in coding +% 2.1 (jan 2012) - modified a few properties, included check is figure +% still exists (after comment on GETKEY on FEX by Andrew). +% check input argument + narginchk(1,1) ; + if numel(m)~=1 || ~isnumeric(m) || ~isfinite(m) || m <= 0, + error('Argument should be a single positive number.') ; + end + % set up the timer + tt = timer ; + tt.timerfcn = 'uiresume' ; + tt.startdelay = m ; + % Set up the figure + % May be the position property should be individually tweaked to avoid visibility + callstr = 'set(gcbf,''Userdata'',double(get(gcbf,''Currentcharacter''))) ; uiresume ' ; + fh = figure(... + 'name','Press a key', ... + 'keypressfcn',callstr, ... + 'windowstyle','modal',... + 'numbertitle','off', ... + 'position',[0 0 1 1],... + 'userdata',-1) ; + try + % Wait for something to happen or the timer to run out + start(tt) ; + uiwait ; + ch = get(fh,'Userdata') ; + if isempty(ch), % a non-ascii key was pressed, return a NaN + ch = NaN ; + end + catch + % Something went wrong, return zero. + ch = 0 ; + end + % clean up the timer ... + stop(tt) ; + delete(tt) ; + % ... and figure + if ishandle(fh) + delete(fh) ; + end +end \ No newline at end of file diff --git a/ros/src/dynamixel_sdk/__init__.py b/python/CATKIN_IGNORE similarity index 100% rename from ros/src/dynamixel_sdk/__init__.py rename to python/CATKIN_IGNORE diff --git a/python/setup.py b/python/setup.py index c10954e7..54836fa5 100644 --- a/python/setup.py +++ b/python/setup.py @@ -4,7 +4,7 @@ setup( name='dynamixel_sdk', - version='3.6.0', + version='3.7.41', packages=['dynamixel_sdk'], package_dir={'': 'src'}, license='Apache 2.0', diff --git a/python/src/dynamixel_sdk/port_handler.py b/python/src/dynamixel_sdk/port_handler.py index de555d45..6bec0a93 100644 --- a/python/src/dynamixel_sdk/port_handler.py +++ b/python/src/dynamixel_sdk/port_handler.py @@ -129,14 +129,8 @@ def setupPort(self, cflag_baud): return True def getCFlagBaud(self, baudrate): - if platform.system() == 'Darwin': - if baudrate in [9600, 19200, 38400, 57600, 115200]: - return baudrate - else: - return -1 + if baudrate in [9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1152000, + 2000000, 2500000, 3000000, 3500000, 4000000]: + return baudrate else: - if baudrate in [9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1152000, - 2000000, 2500000, 3000000, 3500000, 4000000]: - return baudrate - else: - return -1 + return -1 diff --git a/python/src/dynamixel_sdk/protocol2_packet_handler.py b/python/src/dynamixel_sdk/protocol2_packet_handler.py index a3df5ea8..6b2fd690 100644 --- a/python/src/dynamixel_sdk/protocol2_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol2_packet_handler.py @@ -21,8 +21,8 @@ from .robotis_def import * -TXPACKET_MAX_LEN = 4 * 1024 -RXPACKET_MAX_LEN = 4 * 1024 +TXPACKET_MAX_LEN = 1 * 1024 +RXPACKET_MAX_LEN = 1 * 1024 # for Protocol 2.0 Packet PKT_HEADER0 = 0 @@ -197,10 +197,9 @@ def removeStuffing(self, packet): packet[i + PKT_INSTRUCTION - 1] == 0xFF) and (packet[i + PKT_INSTRUCTION - 2] == 0xFF): # FF FF FD FD packet_length_out = packet_length_out - 1 - i += 1 - - packet[index] = packet[i + PKT_INSTRUCTION] - index += 1 + else: + packet[index] = packet[i + PKT_INSTRUCTION] + index += 1 packet[index] = packet[PKT_INSTRUCTION + packet_length_in - 2] packet[index + 1] = packet[PKT_INSTRUCTION + packet_length_in - 1] @@ -384,6 +383,8 @@ def broadcastPing(self, port): txpacket = [0] * 10 rxpacket = [] + tx_time_per_byte = (1000.0 / port.getBaudRate()) *10.0; + txpacket[PKT_ID] = BROADCAST_ID txpacket[PKT_LENGTH_L] = 3 txpacket[PKT_LENGTH_H] = 0 @@ -395,7 +396,8 @@ def broadcastPing(self, port): return data_list, result # set rx timeout - port.setPacketTimeout(wait_length * 1) + #port.setPacketTimeout(wait_length * 1) + port.setPacketTimeoutMillis((wait_length * tx_time_per_byte) + (3.0 * MAX_ID) + 16.0); while True: rxpacket += port.readPort(wait_length - rx_length) @@ -472,6 +474,22 @@ def reboot(self, port, dxl_id): _, result, error = self.txRxPacket(port, txpacket) return result, error + def clearMultiTurn(self, port, dxl_id): + txpacket = [0] * 15 + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = 8 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_CLEAR + txpacket[PKT_PARAMETER0 + 0] = 0x01 + txpacket[PKT_PARAMETER0 + 1] = 0x44 + txpacket[PKT_PARAMETER0 + 2] = 0x58 + txpacket[PKT_PARAMETER0 + 3] = 0x4C + txpacket[PKT_PARAMETER0 + 4] = 0x22 + + _, result, error = self.txRxPacket(port, txpacket) + return result, error + def factoryReset(self, port, dxl_id, option): txpacket = [0] * 11 diff --git a/python/src/dynamixel_sdk/robotis_def.py b/python/src/dynamixel_sdk/robotis_def.py index aacadd76..b618b38b 100644 --- a/python/src/dynamixel_sdk/robotis_def.py +++ b/python/src/dynamixel_sdk/robotis_def.py @@ -29,6 +29,7 @@ INST_REG_WRITE = 4 INST_ACTION = 5 INST_FACTORY_RESET = 6 +INST_CLEAR = 16 INST_SYNC_WRITE = 131 # 0x83 INST_BULK_READ = 146 # 0x92 # --- Only for 2.0 --- diff --git a/python/tests/protocol1_0/read_write.py b/python/tests/protocol1_0/read_write.py index 2d8fa88e..099770d5 100644 --- a/python/tests/protocol1_0/read_write.py +++ b/python/tests/protocol1_0/read_write.py @@ -116,7 +116,7 @@ def getch(): break # Write goal position - dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]) + dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: @@ -124,7 +124,7 @@ def getch(): while 1: # Read present position - dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_MX_PRESENT_POSITION) + dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read2ByteTxRx(portHandler, DXL_ID, ADDR_MX_PRESENT_POSITION) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: diff --git a/python/tests/protocol2_0/bulk_read_write.py b/python/tests/protocol2_0/bulk_read_write.py index 651f32fb..5443a1f0 100644 --- a/python/tests/protocol2_0/bulk_read_write.py +++ b/python/tests/protocol2_0/bulk_read_write.py @@ -124,7 +124,7 @@ def getch(): else: print("Dynamixel#%d has been successfully connected" % DXL1_ID) -# Enable Dynamixel#1 Torque +# Enable Dynamixel#2 Torque dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) diff --git a/python/tests/protocol2_0/clear_multi_turn.py b/python/tests/protocol2_0/clear_multi_turn.py new file mode 100644 index 00000000..df35d4fc --- /dev/null +++ b/python/tests/protocol2_0/clear_multi_turn.py @@ -0,0 +1,215 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +################################################################################ +# Copyright 2017 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +################################################################################ + +# Author: Ki Jong Gil (Gilbert) + +# +# ********* Clear Multi Turn Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 2.0 +# This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +# To use another Dynamixel model, such as X series, see their details in E-Manual(emanual.robotis.com) and edit below variables yourself. +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + +from __future__ import print_function +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import termios, fcntl, sys, os + from select import select + fd = sys.stdin.fileno() + old_term = termios.tcgetattr(fd) + new_term = termios.tcgetattr(fd) + + def getch(): + new_term[3] = (new_term[3] & ~termios.ICANON & ~termios.ECHO) + termios.tcsetattr(fd, termios.TCSANOW, new_term) + try: + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_term) + return ch + + def kbhit(): + new_term[3] = (new_term[3] & ~(termios.ICANON | termios.ECHO)) + termios.tcsetattr(fd, termios.TCSANOW, new_term) + try: + dr,dw,de = select([sys.stdin], [], [], 0) + if dr != []: + return 1 + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_term) + sys.stdout.flush() + + return 0 + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_OPERATING_MODE = 11 # Control table address is different in Dynamixel model +ADDR_TORQUE_ENABLE = 64 +ADDR_GOAL_POSITION = 116 +ADDR_PRESENT_POSITION = 132 + +# Protocol version +PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel + +# Default setting +DXL_ID = 1 # Dynamixel ID : 1 +BAUDRATE = 57600 # Dynamixel default baudrate : 57600 +DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +TORQUE_ENABLE = 1 # Value for enabling the torque +TORQUE_DISABLE = 0 # Value for disabling the torque +MAX_POSITION_VALUE = 1048575 +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel will rotate between this value +EXT_POSITION_CONTROL_MODE = 4 # Value for extended position control mode (operating mode) + +ESC_ASCII_VALUE = 0x1b +SPACE_ASCII_VALUE = 0x20 + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Set the protocol version +# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler +packetHandler = PacketHandler(PROTOCOL_VERSION) + +# Open port +if portHandler.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + quit() + + +# Set port baudrate +if portHandler.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") +else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + getch() + quit() + +# Set operating mode to extended position control mode +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +else: + print("Operating mode changed to extended position control mode.") + +# Enable Dynamixel Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +else: + print("Dynamixel has been successfully connected") + +while 1: + print("\nPress any key to continue! (or press ESC to quit!)") + if getch() == chr(ESC_ASCII_VALUE): + break + print(" Press SPACE key to clear multi-turn information! (or press ESC to stop!)") + + # Write goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, MAX_POSITION_VALUE) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + while 1: + # Read present position + dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + print(" [ID:%03d] GoalPos:%03d PresPos:%03d" %(DXL_ID, MAX_POSITION_VALUE, dxl_present_position), end = "\r") + if kbhit(): + c = getch() + if c == chr(SPACE_ASCII_VALUE): + print("\n Stop & Clear Multi-Turn Information! ") + # Write the present position to the goal position to stop moving + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, dxl_present_position) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + time.sleep(0.3) + + # Clear Multi-Turn Information + dxl_comm_result, dxl_error = packetHandler.clearMultiTurn(portHandler, DXL_ID) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + # Read present position + dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + print(" Present Position has been reset. : %03d" % dxl_present_position) + + break + + elif c == chr(ESC_ASCII_VALUE): + print("\n Stopped!!") + # Write the present position to the goal position to stop moving + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, dxl_present_position) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + break + + if not abs(MAX_POSITION_VALUE - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD: + break + +# Disable Dynamixel Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + +# Close port +portHandler.closePort() diff --git a/ros/CHANGELOG.rst b/ros/dynamixel_sdk/CHANGELOG.rst similarity index 81% rename from ros/CHANGELOG.rst rename to ros/dynamixel_sdk/CHANGELOG.rst index 829e486d..1cea7ea2 100644 --- a/ros/CHANGELOG.rst +++ b/ros/dynamixel_sdk/CHANGELOG.rst @@ -2,12 +2,52 @@ Changelog for package dynamixel_sdk ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.7.41(2020-08-12) +------------------- +* DYNAMIXEL SDK ROS example initial commit. +* Bug fix for 4.5Mbps support (#430) +* Contributors: Zerom, Will Son + +3.7.31 (2020-07-13) +------------------- +* ROS 1 Noetic Ninjemys support +* 3x faster getError member function of GroupSyncRead Class +* Contributors: developer0hye, Zerom, Will Son + +3.7.21 (2019-09-06) +------------------- +* Fixed buffer overflow bug (rxpacket size) +* Fixed typo in the package.xml and header files +* Contributors: Chris Lalancette, Zerom, Pyo + +3.7.11 (2019-08-19) +------------------- +- Updated C lib and DLL file +- Changed C# / win32 / protocol_combined output path +- Fixed "protocol_combined" example bug +- Fixed typo in bulk_read_write.py +* Contributors: Pyo, Zerom + +3.7.10 (2019-08-19) +------------------- +* Supported ROS 2 Dashing Diademata +* Contributors: Darby, Pyo + +3.7.0 (2019-01-03) +------------------ +* Added clear instruction `#269 `_ +* Removed busy waiting for rxPacket() +* Fixed addStuffing() function (reduced stack memory usage) +* Fixed memory issues `#268 `_ +* Fixed the broadcast ping bug in dxl_monitor +* Contributors: Gilbert, Pyo, Zerom + 3.6.2 (2018-07-17) ------------------ -* added python modules for ROS to ros folder -* moved cpp library files for ROS to ros folder -* created an ROS package separately `#187 `_ -* modified the e-Manual address to emanual.robotis.com +* Added python modules for ROS to ros folder +* Moved cpp library files for ROS to ros folder +* Created an ROS package separately `#187 `_ +* Modified the e-Manual address to emanual.robotis.com * Contributors: Pyo 3.6.1 (2018-06-14) diff --git a/ros/CMakeLists.txt b/ros/dynamixel_sdk/CMakeLists.txt similarity index 97% rename from ros/CMakeLists.txt rename to ros/dynamixel_sdk/CMakeLists.txt index f71e5443..768bbe31 100644 --- a/ros/CMakeLists.txt +++ b/ros/dynamixel_sdk/CMakeLists.txt @@ -4,6 +4,9 @@ cmake_minimum_required(VERSION 2.8.3) project(dynamixel_sdk) +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + ################################################################################ # Find catkin packages and libraries for catkin and system dependencies ################################################################################ diff --git a/ros/include/dynamixel_sdk/dynamixel_sdk.h b/ros/dynamixel_sdk/include/dynamixel_sdk/dynamixel_sdk.h similarity index 100% rename from ros/include/dynamixel_sdk/dynamixel_sdk.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/dynamixel_sdk.h diff --git a/ros/include/dynamixel_sdk/group_bulk_read.h b/ros/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h similarity index 100% rename from ros/include/dynamixel_sdk/group_bulk_read.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h diff --git a/ros/include/dynamixel_sdk/group_bulk_write.h b/ros/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h similarity index 100% rename from ros/include/dynamixel_sdk/group_bulk_write.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h diff --git a/ros/include/dynamixel_sdk/group_sync_read.h b/ros/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h similarity index 100% rename from ros/include/dynamixel_sdk/group_sync_read.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h diff --git a/ros/include/dynamixel_sdk/group_sync_write.h b/ros/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h similarity index 100% rename from ros/include/dynamixel_sdk/group_sync_write.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h diff --git a/ros/include/dynamixel_sdk/packet_handler.h b/ros/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h similarity index 96% rename from ros/include/dynamixel_sdk/packet_handler.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h index 37ff2400..8b20e26f 100644 --- a/ros/include/dynamixel_sdk/packet_handler.h +++ b/ros/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h @@ -24,6 +24,11 @@ #if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include + +#define ERROR_PRINT SerialBT2.print +#else +#define ERROR_PRINT printf + #endif #include @@ -52,6 +57,7 @@ #define INST_BULK_READ 146 // 0x92 // --- Only for 2.0 --- // #define INST_REBOOT 8 +#define INST_CLEAR 16 // 0x10 #define INST_STATUS 85 // 0x55 #define INST_SYNC_READ 130 // 0x82 #define INST_BULK_WRITE 147 // 0x93 @@ -222,6 +228,19 @@ class WINDECLSPEC PacketHandler //////////////////////////////////////////////////////////////////////////////// virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reset multi-turn revolution information of Dynamixel + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), + /// @description Dynamixel X-series (Firmware v42 or above). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + //////////////////////////////////////////////////////////////////////////////// /// @brief The function that makes Dynamixel reset as it was produced in the factory /// @description The function makes an instruction packet with INST_FACTORY_RESET, @@ -491,7 +510,7 @@ class WINDECLSPEC PacketHandler /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits the packet with PacketHandler::txPacket(). - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write @@ -506,7 +525,7 @@ class WINDECLSPEC PacketHandler /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits and receives the packet with PacketHandler::txRxPacket(), /// @description gets the error from the packet. - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write diff --git a/ros/include/dynamixel_sdk/port_handler.h b/ros/dynamixel_sdk/include/dynamixel_sdk/port_handler.h similarity index 100% rename from ros/include/dynamixel_sdk/port_handler.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/port_handler.h diff --git a/ros/include/dynamixel_sdk/port_handler_arduino.h b/ros/dynamixel_sdk/include/dynamixel_sdk/port_handler_arduino.h similarity index 100% rename from ros/include/dynamixel_sdk/port_handler_arduino.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/port_handler_arduino.h diff --git a/ros/include/dynamixel_sdk/port_handler_linux.h b/ros/dynamixel_sdk/include/dynamixel_sdk/port_handler_linux.h similarity index 100% rename from ros/include/dynamixel_sdk/port_handler_linux.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/port_handler_linux.h diff --git a/ros/include/dynamixel_sdk/port_handler_mac.h b/ros/dynamixel_sdk/include/dynamixel_sdk/port_handler_mac.h similarity index 100% rename from ros/include/dynamixel_sdk/port_handler_mac.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/port_handler_mac.h diff --git a/ros/include/dynamixel_sdk/port_handler_windows.h b/ros/dynamixel_sdk/include/dynamixel_sdk/port_handler_windows.h similarity index 100% rename from ros/include/dynamixel_sdk/port_handler_windows.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/port_handler_windows.h diff --git a/ros/include/dynamixel_sdk/protocol1_packet_handler.h b/ros/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h similarity index 98% rename from ros/include/dynamixel_sdk/protocol1_packet_handler.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h index ebfa05c0..3dcbadd1 100644 --- a/ros/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/ros/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h @@ -178,6 +178,15 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that reset multi-turn revolution information of Dynamixel + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// /// @brief The function that makes Dynamixel reset as it was produced in the factory /// @description The function makes an instruction packet with INST_FACTORY_RESET, @@ -447,7 +456,7 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits the packet with Protocol1PacketHandler::txPacket(). - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write @@ -462,7 +471,7 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits and receives the packet with Protocol1PacketHandler::txRxPacket(), /// @description gets the error from the packet. - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write diff --git a/ros/include/dynamixel_sdk/protocol2_packet_handler.h b/ros/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h similarity index 97% rename from ros/include/dynamixel_sdk/protocol2_packet_handler.h rename to ros/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h index 3c953f39..b8bd8856 100644 --- a/ros/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/ros/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h @@ -186,6 +186,19 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reset multi-turn revolution information of Dynamixel + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). + /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), + /// @description Dynamixel X-series (Firmware v42 or above). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0); + //////////////////////////////////////////////////////////////////////////////// /// @brief The function that makes Dynamixel reset as it was produced in the factory /// @description The function makes an instruction packet with INST_FACTORY_RESET, @@ -455,7 +468,7 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits the packet with Protocol2PacketHandler::txPacket(). - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write @@ -470,7 +483,7 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, /// @description transmits and receives the packet with Protocol2PacketHandler::txRxPacket(), /// @description gets the error from the packet. - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamxel. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. /// @param port PortHandler instance /// @param id Dynamixel ID /// @param address Address of the data for write diff --git a/ros/package.xml b/ros/dynamixel_sdk/package.xml similarity index 57% rename from ros/package.xml rename to ros/dynamixel_sdk/package.xml index a24119b7..6d8c066c 100644 --- a/ros/package.xml +++ b/ros/dynamixel_sdk/package.xml @@ -1,15 +1,16 @@ dynamixel_sdk - 3.6.2 + 3.7.41 - This package is wrapping version of ROBOTIS Dynamxel SDK for ROS. The ROBOTIS Dynamixel SDK, or SDK, is a software development library that provides Dynamixel control functions for packet communication. The API is designed for Dynamixel actuators and Dynamixel-based platforms. + This package is wrapping version of ROBOTIS Dynamixel SDK for ROS. The ROBOTIS Dynamixel SDK, or SDK, is a software development library that provides Dynamixel control functions for packet communication. The API is designed for Dynamixel actuators and Dynamixel-based platforms. Apache 2.0 - Pyo + Gilbert Zerom + Darby Lim Leon - Pyo + Will Son http://wiki.ros.org/dynamixel_sdk http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/ https://github.com/ROBOTIS-GIT/DynamixelSDK diff --git a/ros/setup.py b/ros/dynamixel_sdk/setup.py similarity index 100% rename from ros/setup.py rename to ros/dynamixel_sdk/setup.py diff --git a/ros/src/DynamixelSDK.h b/ros/dynamixel_sdk/src/DynamixelSDK.h similarity index 100% rename from ros/src/DynamixelSDK.h rename to ros/dynamixel_sdk/src/DynamixelSDK.h diff --git a/ros/dynamixel_sdk/src/dynamixel_sdk/__init__.py b/ros/dynamixel_sdk/src/dynamixel_sdk/__init__.py new file mode 100644 index 00000000..bc227b66 --- /dev/null +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/__init__.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +################################################################################ +# Copyright 2017 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +################################################################################ + +# Author: Ryu Woon Jung (Leon) + +from .port_handler import * +from .packet_handler import * +from .group_sync_read import * +from .group_sync_write import * +from .group_bulk_read import * +from .group_bulk_write import * diff --git a/ros/src/dynamixel_sdk/group_bulk_read.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp similarity index 98% rename from ros/src/dynamixel_sdk/group_bulk_read.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp index bcea7551..edd1ad69 100644 --- a/ros/src/dynamixel_sdk/group_bulk_read.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp @@ -232,14 +232,5 @@ bool GroupBulkRead::getError(uint8_t id, uint8_t* error) // TODO : check protocol version, last_result_, data_list // if (last_result_ == false || error_list_.find(id) == error_list_.end()) - error[0] = error_list_[id][0]; - - if (error[0] != 0) - { - return true; - } - else - { - return false; - } + return error[0] = error_list_[id][0]; } \ No newline at end of file diff --git a/ros/src/dynamixel_sdk/group_bulk_read.py b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.py similarity index 100% rename from ros/src/dynamixel_sdk/group_bulk_read.py rename to ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.py diff --git a/ros/src/dynamixel_sdk/group_bulk_write.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp similarity index 100% rename from ros/src/dynamixel_sdk/group_bulk_write.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp diff --git a/ros/src/dynamixel_sdk/group_bulk_write.py b/ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.py similarity index 100% rename from ros/src/dynamixel_sdk/group_bulk_write.py rename to ros/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.py diff --git a/ros/src/dynamixel_sdk/group_sync_read.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp similarity index 97% rename from ros/src/dynamixel_sdk/group_sync_read.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp index f392943b..79e5f3b8 100644 --- a/ros/src/dynamixel_sdk/group_sync_read.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp @@ -201,14 +201,5 @@ bool GroupSyncRead::getError(uint8_t id, uint8_t* error) // TODO : check protocol version, last_result_, data_list // if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || error_list_.find(id) == error_list_.end()) - error[0] = error_list_[id][0]; - - if (error[0] != 0) - { - return true; - } - else - { - return false; - } + return error[0] = error_list_[id][0]; } \ No newline at end of file diff --git a/ros/src/dynamixel_sdk/group_sync_read.py b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.py similarity index 100% rename from ros/src/dynamixel_sdk/group_sync_read.py rename to ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.py diff --git a/ros/src/dynamixel_sdk/group_sync_write.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp similarity index 100% rename from ros/src/dynamixel_sdk/group_sync_write.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp diff --git a/ros/src/dynamixel_sdk/group_sync_write.py b/ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.py similarity index 100% rename from ros/src/dynamixel_sdk/group_sync_write.py rename to ros/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.py diff --git a/ros/src/dynamixel_sdk/packet_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp similarity index 100% rename from ros/src/dynamixel_sdk/packet_handler.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp diff --git a/ros/src/dynamixel_sdk/packet_handler.py b/ros/dynamixel_sdk/src/dynamixel_sdk/packet_handler.py similarity index 100% rename from ros/src/dynamixel_sdk/packet_handler.py rename to ros/dynamixel_sdk/src/dynamixel_sdk/packet_handler.py diff --git a/ros/src/dynamixel_sdk/port_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp similarity index 100% rename from ros/src/dynamixel_sdk/port_handler.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp diff --git a/ros/src/dynamixel_sdk/port_handler.py b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.py similarity index 100% rename from ros/src/dynamixel_sdk/port_handler.py rename to ros/dynamixel_sdk/src/dynamixel_sdk/port_handler.py diff --git a/ros/src/dynamixel_sdk/port_handler_arduino.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp similarity index 92% rename from ros/src/dynamixel_sdk/port_handler_arduino.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp index 19ad599f..07add9ad 100644 --- a/ros/src/dynamixel_sdk/port_handler_arduino.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp @@ -27,7 +27,7 @@ #define DYNAMIXEL_SERIAL Serial3 #endif -#define LATENCY_TIMER 16 // msec (USB latency timer) +#define LATENCY_TIMER 4 // msec (USB latency timer) using namespace dynamixel; @@ -91,10 +91,17 @@ void PortHandlerArduino::closePort() void PortHandlerArduino::clearPort() { + int temp __attribute__((unused)); #if defined(__OPENCR__) - DYNAMIXEL_SERIAL.flush(); + while (DYNAMIXEL_SERIAL.available()) + { + temp = DYNAMIXEL_SERIAL.read(); + } #elif defined(__OPENCM904__) - p_dxl_serial->flush(); + while (p_dxl_serial->available()) + { + temp = p_dxl_serial->read(); + } #endif } @@ -205,7 +212,7 @@ bool PortHandlerArduino::isPacketTimeout() double PortHandlerArduino::getCurrentTime() { - return (double)millis(); + return (double)millis(); } double PortHandlerArduino::getTimeSinceStart() @@ -285,8 +292,15 @@ void PortHandlerArduino::setTxEnable() void PortHandlerArduino::setTxDisable() { #if defined(__OPENCR__) +#ifdef SERIAL_WRITES_NON_BLOCKING + DYNAMIXEL_SERIAL.flush(); // make sure it completes before we disable... +#endif drv_dxl_tx_enable(FALSE); + #elif defined(__OPENCM904__) +#ifdef SERIAL_WRITES_NON_BLOCKING + p_dxl_serial->flush(); +#endif drv_dxl_tx_enable(socket_fd_, FALSE); #endif } diff --git a/ros/src/dynamixel_sdk/port_handler_linux.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp similarity index 89% rename from ros/src/dynamixel_sdk/port_handler_linux.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp index 57564a94..5ba4091d 100644 --- a/ros/src/dynamixel_sdk/port_handler_linux.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp @@ -55,6 +55,27 @@ // or if you have another good idea that can be an alternatives, // please give us advice via github issue https://github.com/ROBOTIS-GIT/DynamixelSDK/issues +struct termios2 { + tcflag_t c_iflag; /* input mode flags */ + tcflag_t c_oflag; /* output mode flags */ + tcflag_t c_cflag; /* control mode flags */ + tcflag_t c_lflag; /* local mode flags */ + cc_t c_line; /* line discipline */ + cc_t c_cc[19]; /* control characters */ + speed_t c_ispeed; /* input speed */ + speed_t c_ospeed; /* output speed */ +}; + +#ifndef TCGETS2 +#define TCGETS2 _IOR('T', 0x2A, struct termios2) +#endif +#ifndef TCSETS2 +#define TCSETS2 _IOW('T', 0x2B, struct termios2) +#endif +#ifndef BOTHER +#define BOTHER 0010000 +#endif + using namespace dynamixel; PortHandlerLinux::PortHandlerLinux(const char *port_name) @@ -207,6 +228,19 @@ bool PortHandlerLinux::setupPort(int cflag_baud) bool PortHandlerLinux::setCustomBaudrate(int speed) { + struct termios2 options; + + if (ioctl(socket_fd_, TCGETS2, &options) != 01) + { + options.c_cflag &= ~CBAUD; + options.c_cflag |= BOTHER; + options.c_ispeed = speed; + options.c_ospeed = speed; + + if (ioctl(socket_fd_, TCSETS2, &options) != -1) + return true; + } + // try to set a custom divisor struct serial_struct ss; if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) diff --git a/ros/src/dynamixel_sdk/port_handler_mac.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp similarity index 100% rename from ros/src/dynamixel_sdk/port_handler_mac.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp diff --git a/ros/src/dynamixel_sdk/port_handler_windows.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp similarity index 100% rename from ros/src/dynamixel_sdk/port_handler_windows.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp diff --git a/ros/src/dynamixel_sdk/protocol1_packet_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp similarity index 97% rename from ros/src/dynamixel_sdk/protocol1_packet_handler.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp index f8efe111..668adfa3 100644 --- a/ros/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -365,6 +365,11 @@ int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error return COMM_NOT_AVAILABLE; } +int Protocol1PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error) +{ + return COMM_NOT_AVAILABLE; +} + int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) { uint8_t txpacket[6] = {0}; @@ -407,6 +412,9 @@ int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t lengt uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length+6); //uint8_t *rxpacket = new uint8_t[length+6]; + if (rxpacket == NULL) + return result; + do { result = rxPacket(port, rxpacket); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); @@ -436,8 +444,14 @@ int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add uint8_t txpacket[8] = {0}; uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); + if (rxpacket == NULL) + return result; + if (id >= BROADCAST_ID) + { + free(rxpacket); return COMM_NOT_AVAILABLE; + } txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = 4; @@ -534,6 +548,9 @@ int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t uint8_t *txpacket = (uint8_t *)malloc(length+7); //uint8_t *txpacket = new uint8_t[length+7]; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_WRITE; @@ -559,6 +576,9 @@ int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad //uint8_t *txpacket = new uint8_t[length+7]; uint8_t rxpacket[6] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_WRITE; @@ -615,6 +635,9 @@ int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 uint8_t *txpacket = (uint8_t *)malloc(length+6); //uint8_t *txpacket = new uint8_t[length+6]; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; @@ -640,6 +663,9 @@ int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t //uint8_t *txpacket = new uint8_t[length+6]; uint8_t rxpacket[6] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH] = length+3; txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; @@ -669,6 +695,9 @@ int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM //uint8_t *txpacket = new uint8_t[param_length + 8]; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; @@ -694,6 +723,9 @@ int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM //uint8_t *txpacket = new uint8_t[param_length + 7]; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM txpacket[PKT_INSTRUCTION] = INST_BULK_READ; diff --git a/ros/src/dynamixel_sdk/protocol1_packet_handler.py b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.py similarity index 100% rename from ros/src/dynamixel_sdk/protocol1_packet_handler.py rename to ros/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.py diff --git a/ros/src/dynamixel_sdk/protocol2_packet_handler.cpp b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp similarity index 90% rename from ros/src/dynamixel_sdk/protocol2_packet_handler.cpp rename to ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp index c897e620..06dae1d2 100644 --- a/ros/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -17,11 +17,14 @@ /* Author: zerom, Ryu Woon Jung (Leon) */ #if defined(__linux__) +#include #include "protocol2_packet_handler.h" #elif defined(__APPLE__) +#include #include "protocol2_packet_handler.h" #elif defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT +#include #include "protocol2_packet_handler.h" #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include "../../include/dynamixel_sdk/protocol2_packet_handler.h" @@ -31,8 +34,8 @@ #include #include -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*1024) +#define TXPACKET_MAX_LEN (1*1024) +#define RXPACKET_MAX_LEN (1*1024) ///////////////// for Protocol 2.0 Packet ///////////////// #define PKT_HEADER0 0 @@ -140,7 +143,7 @@ const char *Protocol2PacketHandler::getRxPacketError(uint8_t error) unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) { uint16_t i; - uint16_t crc_table[256] = {0x0000, + static const uint16_t crc_table[256] = {0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, @@ -190,39 +193,48 @@ unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *da void Protocol2PacketHandler::addStuffing(uint8_t *packet) { - int i = 0, index = 0; int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); int packet_length_out = packet_length_in; - uint8_t temp[TXPACKET_MAX_LEN] = {0}; + + if (packet_length_in < 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD + return; - for (uint16_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) - temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC + uint8_t *packet_ptr; + uint16_t packet_length_before_crc = packet_length_in - 2; + for (uint16_t i = 3; i < packet_length_before_crc; i++) { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; + packet_ptr = &packet[i+PKT_INSTRUCTION-2]; + if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) packet_length_out++; + } + + if (packet_length_in == packet_length_out) // no stuffing required + return; + + uint16_t out_index = packet_length_out + 6 - 2; // last index before crc + uint16_t in_index = packet_length_in + 6 - 2; // last index before crc + while (out_index != in_index) + { + if (packet[in_index] == 0xFD && packet[in_index-1] == 0xFF && packet[in_index-2] == 0xFF) + { + packet[out_index--] = 0xFD; // byte stuffing + if (out_index != in_index) + { + packet[out_index--] = packet[in_index--]; // FD + packet[out_index--] = packet[in_index--]; // FF + packet[out_index--] = packet[in_index--]; // FF + } + } + else + { + packet[out_index--] = packet[in_index--]; } } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - - ////////////////////////// - if (packet_length_in != packet_length_out) - packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - /////////////////////////// - - for (uint16_t s = 0; s < index; s++) - packet[s] = temp[s]; - //memcpy(packet, temp, index); packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + + return; } void Protocol2PacketHandler::removeStuffing(uint8_t *packet) @@ -393,6 +405,11 @@ int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) break; } } +#if defined(__linux__) || defined(__APPLE__) + usleep(0); +#elif defined(_WIN32) || defined(_WIN64) + Sleep(0); +#endif } port->is_using_ = false; @@ -489,6 +506,8 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vectorgetBaudRate()) * 10.0; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = 3; txpacket[PKT_LENGTH_H] = 0; @@ -502,7 +521,8 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vectorsetPacketTimeout((uint16_t)(wait_length * 30)); + //port->setPacketTimeout((uint16_t)(wait_length * 30)); + port->setPacketTimeout(((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0); while(1) { @@ -595,6 +615,24 @@ int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error return txRxPacket(port, txpacket, rxpacket, error); } +int Protocol2PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error) +{ + uint8_t txpacket[15] = {0}; + uint8_t rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 8; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_CLEAR; + txpacket[PKT_PARAMETER0] = 0x01; + txpacket[PKT_PARAMETER0+1] = 0x44; + txpacket[PKT_PARAMETER0+2] = 0x58; + txpacket[PKT_PARAMETER0+3] = 0x4C; + txpacket[PKT_PARAMETER0+4] = 0x22; + + return txRxPacket(port, txpacket, rxpacket, error); +} + int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) { uint8_t txpacket[11] = {0}; @@ -641,8 +679,10 @@ int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t lengt int result = COMM_TX_FAIL; uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length + 11 + (length/3)); // (length/3): consider stuffing - //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing - + + if (rxpacket == NULL) + return result; + do { result = rxPacket(port, rxpacket); } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); @@ -672,8 +712,14 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length + 11 + (length/3)); // (length/3): consider stuffing + if (rxpacket == NULL) + return result; + if (id >= BROADCAST_ID) + { + free(rxpacket); return COMM_NOT_AVAILABLE; + } txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = 7; @@ -770,8 +816,10 @@ int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + + if (txpacket == NULL) + return result; txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); @@ -796,10 +844,12 @@ int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t ad { int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -853,11 +903,13 @@ int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -879,12 +931,14 @@ int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16 int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12); - //uint8_t *txpacket = new uint8_t[length+12]; + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); uint8_t rxpacket[11] = {0}; + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = id; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); @@ -905,11 +959,14 @@ int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -933,12 +990,14 @@ int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14); - //uint8_t *txpacket = new uint8_t[param_length + 14]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H @@ -961,12 +1020,14 @@ int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_ad int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H @@ -992,12 +1053,14 @@ int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16 int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10); - //uint8_t *txpacket = new uint8_t[param_length + 10]; + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + if (txpacket == NULL) + return result; + txpacket[PKT_ID] = BROADCAST_ID; txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H diff --git a/ros/src/dynamixel_sdk/protocol2_packet_handler.py b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.py similarity index 97% rename from ros/src/dynamixel_sdk/protocol2_packet_handler.py rename to ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.py index a3df5ea8..6b2fd690 100644 --- a/ros/src/dynamixel_sdk/protocol2_packet_handler.py +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.py @@ -21,8 +21,8 @@ from .robotis_def import * -TXPACKET_MAX_LEN = 4 * 1024 -RXPACKET_MAX_LEN = 4 * 1024 +TXPACKET_MAX_LEN = 1 * 1024 +RXPACKET_MAX_LEN = 1 * 1024 # for Protocol 2.0 Packet PKT_HEADER0 = 0 @@ -197,10 +197,9 @@ def removeStuffing(self, packet): packet[i + PKT_INSTRUCTION - 1] == 0xFF) and (packet[i + PKT_INSTRUCTION - 2] == 0xFF): # FF FF FD FD packet_length_out = packet_length_out - 1 - i += 1 - - packet[index] = packet[i + PKT_INSTRUCTION] - index += 1 + else: + packet[index] = packet[i + PKT_INSTRUCTION] + index += 1 packet[index] = packet[PKT_INSTRUCTION + packet_length_in - 2] packet[index + 1] = packet[PKT_INSTRUCTION + packet_length_in - 1] @@ -384,6 +383,8 @@ def broadcastPing(self, port): txpacket = [0] * 10 rxpacket = [] + tx_time_per_byte = (1000.0 / port.getBaudRate()) *10.0; + txpacket[PKT_ID] = BROADCAST_ID txpacket[PKT_LENGTH_L] = 3 txpacket[PKT_LENGTH_H] = 0 @@ -395,7 +396,8 @@ def broadcastPing(self, port): return data_list, result # set rx timeout - port.setPacketTimeout(wait_length * 1) + #port.setPacketTimeout(wait_length * 1) + port.setPacketTimeoutMillis((wait_length * tx_time_per_byte) + (3.0 * MAX_ID) + 16.0); while True: rxpacket += port.readPort(wait_length - rx_length) @@ -472,6 +474,22 @@ def reboot(self, port, dxl_id): _, result, error = self.txRxPacket(port, txpacket) return result, error + def clearMultiTurn(self, port, dxl_id): + txpacket = [0] * 15 + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = 8 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_CLEAR + txpacket[PKT_PARAMETER0 + 0] = 0x01 + txpacket[PKT_PARAMETER0 + 1] = 0x44 + txpacket[PKT_PARAMETER0 + 2] = 0x58 + txpacket[PKT_PARAMETER0 + 3] = 0x4C + txpacket[PKT_PARAMETER0 + 4] = 0x22 + + _, result, error = self.txRxPacket(port, txpacket) + return result, error + def factoryReset(self, port, dxl_id, option): txpacket = [0] * 11 diff --git a/ros/src/dynamixel_sdk/robotis_def.py b/ros/dynamixel_sdk/src/dynamixel_sdk/robotis_def.py similarity index 97% rename from ros/src/dynamixel_sdk/robotis_def.py rename to ros/dynamixel_sdk/src/dynamixel_sdk/robotis_def.py index aacadd76..3986393b 100644 --- a/ros/src/dynamixel_sdk/robotis_def.py +++ b/ros/dynamixel_sdk/src/dynamixel_sdk/robotis_def.py @@ -32,7 +32,8 @@ INST_SYNC_WRITE = 131 # 0x83 INST_BULK_READ = 146 # 0x92 # --- Only for 2.0 --- -INST_REBOOT = 8 +INST_REBOOT = 8 +INST_CLEAR = 16 #0x10 INST_STATUS = 85 # 0x55 INST_SYNC_READ = 130 # 0x82 INST_BULK_WRITE = 147 # 0x93 diff --git a/ros/dynamixel_sdk_examples/CHANGELOG.rst b/ros/dynamixel_sdk_examples/CHANGELOG.rst new file mode 100644 index 00000000..bc7479c4 --- /dev/null +++ b/ros/dynamixel_sdk_examples/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package dynamixel_sdk_examples +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.7.41(2020-08-12) +------------------- +* DYNAMIXEL SDK ROS example initial commit. +* Bug fix for 4.5Mbps support (#430) +* Contributors: Zerom, Will Son diff --git a/ros/dynamixel_sdk_examples/CMakeLists.txt b/ros/dynamixel_sdk_examples/CMakeLists.txt new file mode 100644 index 00000000..0e4d8405 --- /dev/null +++ b/ros/dynamixel_sdk_examples/CMakeLists.txt @@ -0,0 +1,73 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 2.8.3) +project(dynamixel_sdk_examples) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +################################################################################ +# Find catkin packages and libraries for catkin and system dependencies +################################################################################ +find_package(catkin REQUIRED + COMPONENTS + roscpp + std_msgs + message_generation + dynamixel_sdk +) + +################################################################################ +# Declare ROS messages, services and actions +################################################################################ +## Generate messages in the 'msg' folder +add_message_files( + FILES + SetPosition.msg +) + +## Generate services in the 'srv' folder +add_service_files( + FILES + GetPosition.srv +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################################################################ +# Declare catkin specific configuration to be passed to dependent projects +################################################################################ +catkin_package( + CATKIN_DEPENDS + roscpp + std_msgs + dynamixel_sdk +# DEPENDS system_lib +) + +################################################################################ +# Build +################################################################################ +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(read_write_node src/read_write_node.cpp) +add_dependencies(read_write_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(read_write_node ${catkin_LIBRARIES}) + +################################################################################ +# Install +################################################################################ +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS read_write_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/ros/dynamixel_sdk_examples/msg/SetPosition.msg b/ros/dynamixel_sdk_examples/msg/SetPosition.msg new file mode 100644 index 00000000..10fc14b5 --- /dev/null +++ b/ros/dynamixel_sdk_examples/msg/SetPosition.msg @@ -0,0 +1,2 @@ +uint8 id +int32 position diff --git a/ros/dynamixel_sdk_examples/package.xml b/ros/dynamixel_sdk_examples/package.xml new file mode 100644 index 00000000..fa69ffeb --- /dev/null +++ b/ros/dynamixel_sdk_examples/package.xml @@ -0,0 +1,24 @@ + + + dynamixel_sdk_examples + 0.1.0 + The DYNAMIXEL SDK ROS example package + + zerom --> + zerom + + Apache 2.0 + + + + + + + catkin + roscpp + std_msgs + dynamixel_sdk + message_generation + message_runtime + + diff --git a/ros/dynamixel_sdk_examples/src/read_write_node.cpp b/ros/dynamixel_sdk_examples/src/read_write_node.cpp new file mode 100644 index 00000000..d0cc563f --- /dev/null +++ b/ros/dynamixel_sdk_examples/src/read_write_node.cpp @@ -0,0 +1,126 @@ +/******************************************************************************* +* Copyright 2020 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/******************************************************************************* + * This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series with U2D2. + * For other series, please refer to the product eManual and modify the Control Table addresses and other definitions. + * To test this example, please follow the commands below. + * + * Open terminal #1 + * $ roscore + * + * Open terminal #2 + * $ rosrun dynamixel_sdk_examples read_write_node + * + * Open terminal #3 (run one of below commands at a time) + * $ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 0}" + * $ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 1000}" + * $ rosservice call /get_position "id: 1" + * + * Author: Zerom +*******************************************************************************/ + +#include + +#include "std_msgs/String.h" +#include "dynamixel_sdk_examples/GetPosition.h" +#include "dynamixel_sdk_examples/SetPosition.h" +#include "dynamixel_sdk/dynamixel_sdk.h" + +using namespace dynamixel; + +// Control table address +#define ADDR_TORQUE_ENABLE 64 +#define ADDR_GOAL_POSITION 116 +#define ADDR_PRESENT_POSITION 132 + +// Protocol version +#define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series. + +// Default setting +#define DXL1_ID 1 // DXL1 ID +#define DXL2_ID 2 // DXL2 ID +#define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series +#define DEVICE_NAME "/dev/ttyUSB0" // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command + +PortHandler *portHandler; +PacketHandler *packetHandler; + +bool getPresentPosition(dynamixel_sdk_examples::GetPosition::Request &req, + dynamixel_sdk_examples::GetPosition::Response &res) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = COMM_TX_FAIL; + + // Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(int16_t) for the Position Value. + int32_t pos = 0; + + // Read Present Position (length : 4 bytes) and Convert uint32 -> int32 + // When reading 2 byte data from AX / MX(1.0), use read2ByteTxRx() instead. + dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, (uint8_t)req.id, ADDR_PRESENT_POSITION, (uint32_t*)&pos, &dxl_error); + ROS_INFO("getPosition : [ID:%d] -> [POSITION:%d]", req.id, pos); + + res.position = pos; + return true; +} + +void setPositionCallback(const dynamixel_sdk_examples::SetPosition::ConstPtr &msg) +{ + uint8_t dxl_error = 0; + + // Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value. + uint32_t pos = (unsigned int)msg->position; // Convert int32 -> uint32 + + // Write Goal Position (length : 4 bytes) + // When writing 2 byte data to AX / MX(1.0), use write2ByteTxRx() instead. + packetHandler->write4ByteTxRx(portHandler, (uint8_t)msg->id, ADDR_GOAL_POSITION, pos, &dxl_error); + ROS_INFO("setPosition : [ID:%d] [POSITION:%d]", msg->id, msg->position); +} + +int main(int argc, char **argv) +{ + uint8_t dxl_error = 0; + ros::init(argc, argv, "read_write_node"); + ros::NodeHandle nh; + + portHandler = dynamixel::PortHandler::getPortHandler(DEVICE_NAME); + packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION); + + if(portHandler->openPort() == false) + { + ROS_ERROR("Failed to open the port!"); + } + + if(portHandler->setBaudRate(BAUDRATE) == false) + { + ROS_ERROR("Failed to set the baudrate!"); + } + + ros::Subscriber set_position_sub = nh.subscribe("/set_position", 10, setPositionCallback); + ros::ServiceServer get_position_srv = nh.advertiseService("/get_position", getPresentPosition); + + packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error); + packetHandler->write1ByteTxRx(portHandler, DXL2_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error); + + while (ros::ok()) + { + usleep(8 * 1000); + + ros::spin(); + } + + return 0; +} diff --git a/ros/dynamixel_sdk_examples/srv/GetPosition.srv b/ros/dynamixel_sdk_examples/srv/GetPosition.srv new file mode 100644 index 00000000..7441039c --- /dev/null +++ b/ros/dynamixel_sdk_examples/srv/GetPosition.srv @@ -0,0 +1,3 @@ +uint8 id +--- +int32 position