diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 694ca7fa..70edf77d 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -7,14 +7,14 @@ so the work was very hard to be carried out so far. Hence, I will update some GUIDELINES that is very necessary to get your ideas be MERGED. -1. After every release, there is a 'develop' branch. To make your idea be accepted on the next release, +1. After every release, there is a 'develop' branch. To make your idea be accepted on the next release, YOU SHOULD GET PULL REQUEST BASED ON 'DEVELOP' BRANCH, NOT 'MASTER' BRANCH!! All pull requests based on 'master' branch will be 'suspended' or 'won't fix', so make sure before the pull request. -2. I'm hoping that many users can list up on the [CONTRIBUTORS](https://github.com/ROBOTIS-GIT/DynamixelSDK/graphs/contributors). -Seriously, I don't want to get your idea as my name but as your name. However, if your idea is left as based on 'develop branch' or idle, +2. I'm hoping that many users can list up on the [CONTRIBUTORS](https://github.com/ROBOTIS-GIT/DynamixelSDK/graphs/contributors). +Seriously, I don't want to get your idea as my name but as your name. However, if your idea is left as based on 'develop branch' or idle, I can't do anything but upload your idea as my name. Thank you every time again, and let's make the source better to get many users happy while make Dynamixel applications. -2017.12.01 doc ver 1.0.0 +2017.12.01 doc ver 1.0.1 diff --git a/Doxyfile b/Doxyfile index 4ba887c9..b6faa701 100644 --- a/Doxyfile +++ b/Doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "DynamixelSDK c++" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 3.5.4 +PROJECT_NUMBER = 3.6.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/README.md b/README.md index d7f62a70..ac197dbd 100644 --- a/README.md +++ b/README.md @@ -15,8 +15,8 @@ | Dynamixel SDK Version | 1.X | 2.X | 3.X ([Download](https://github.com/ROBOTIS-GIT/DynamixelSDK/releases)) | | ------------- | ------------- | ------------- | ------------- | | Release date | 2010.05.16 | 2015.02.10 | 2016.03.08 | -| Latest version released |||3.5.4| -| |||(2017.12.01)| +| Latest version released |||3.6.0| +| |||(18.03.16)| | OS | Linux | Windows | Linux + Windows + Mac | | Available Dynamixel models | All models | All models | All models | ||||| @@ -34,11 +34,11 @@ | | | MATLAB| MATLAB | | | | LabVIEW| LabVIEW | | | | VB| | -| | | | Python | | | | | Java | | (C++ ver. Library binded)¹| C++| | C++| | | | | ROS | | | | | Arduino | +| (Python ver. Library binded)| Python| | Python| ##### ¹ C++ ver. Library is not optimized in binding other languages. Please use C ver. Library instead. --------------------------------------------------------------------------- diff --git a/ReleaseNote.txt b/ReleaseNote.txt index be4338fe..f7f5c2d1 100644 --- a/ReleaseNote.txt +++ b/ReleaseNote.txt @@ -1,3 +1,17 @@ +============================================== + Dynamixel SDK 3.6.0 (Protocol 1.0/2.0) +============================================== + +- 03.16.2018 + +* Replaced : DynamixelSDK Python as a native language (Python 2 and 3 for Windows, Linux, Mac OS X) #93 #122 #147 #181 #182 #185 +* Added : CONTRIBUTING.md added +* Changes : ISSUE_TEMPLATE.md modified +* Changes : C++ version - SyncRead / BulkRead - getError functions added +* Changes : Deprecated functions removed +* Fixes : DynamixelSDK MATLAB 2017 - new typedef (int8_t / int16_t / int32_t) applied in robotis_def.h #161 #179 +* Fixes : Added missing header file for reset and factory_reset examples #167 + ============================================== Dynamixel SDK 3.5.4 (Protocol 1.0/2.0) ============================================== diff --git a/c#/dynamixel_functions_csharp/win32/dynamixel.cs b/c#/dynamixel_functions_csharp/win32/dynamixel.cs index dc699472..4b98a146 100644 --- a/c#/dynamixel_functions_csharp/win32/dynamixel.cs +++ b/c#/dynamixel_functions_csharp/win32/dynamixel.cs @@ -63,13 +63,9 @@ class dynamixel [DllImport(dll_path)] public static extern void packetHandler (); - [DllImport(dll_path)] - public static extern void printTxRxResult (int protocol_version, int result); [DllImport(dll_path)] public static extern IntPtr getTxRxResult (int protocol_version, int result); [DllImport(dll_path)] - public static extern void printRxPacketError (int protocol_version, byte error); - [DllImport(dll_path)] public static extern IntPtr getRxPacketError (int protocol_version, byte error); [DllImport(dll_path)] diff --git a/c#/dynamixel_functions_csharp/win64/dynamixel.cs b/c#/dynamixel_functions_csharp/win64/dynamixel.cs index f419147a..4c4d912c 100644 --- a/c#/dynamixel_functions_csharp/win64/dynamixel.cs +++ b/c#/dynamixel_functions_csharp/win64/dynamixel.cs @@ -63,13 +63,9 @@ class dynamixel [DllImport(dll_path)] public static extern void packetHandler (); - [DllImport(dll_path)] - public static extern void printTxRxResult (int protocol_version, int result); [DllImport(dll_path)] public static extern IntPtr getTxRxResult (int protocol_version, int result); [DllImport(dll_path)] - public static extern void printRxPacketError (int protocol_version, byte error); - [DllImport(dll_path)] public static extern IntPtr getRxPacketError (int protocol_version, byte error); [DllImport(dll_path)] diff --git a/c++/CHANGELOG.rst b/c++/CHANGELOG.rst index 7e08a1e2..0669204f 100644 --- a/c++/CHANGELOG.rst +++ b/c++/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package dynamixel_sdk ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.6.0 (2018-03-16) +----------- +* Replaced : DynamixelSDK Python as a native language (Python 2 and 3 for Windows, Linux, Mac OS X) #93 #122 #147 #181 #182 #185 +* Added : CONTRIBUTING.md added +* Changes : ISSUE_TEMPLATE.md modified +* Changes : C++ version - SyncRead / BulkRead - getError functions added +* Changes : Deprecated functions removed +* Fixes : DynamixelSDK MATLAB 2017 - new typedef (int8_t / int16_t / int32_t) applied in robotis_def.h #161 #179 +* Fixes : Added missing header file for reset and factory_reset examples #167 +* Contributors: Leon + 3.5.4 (2017-12-01) ----------- * Added : Deprecated is now being shown by attributes #67 #107 diff --git a/c++/example/protocol1.0/bulk_read/bulk_read.cpp b/c++/example/protocol1.0/bulk_read/bulk_read.cpp index b81a166f..41ba565d 100644 --- a/c++/example/protocol1.0/bulk_read/bulk_read.cpp +++ b/c++/example/protocol1.0/bulk_read/bulk_read.cpp @@ -243,7 +243,18 @@ int main() { // Bulkread present position and moving status dxl_comm_result = groupBulkRead.txRxPacket(); - if (dxl_comm_result != COMM_SUCCESS) printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (groupBulkRead.getError(DXL1_ID, &dxl_error)) + { + printf("[ID:%03d] %s\n", DXL1_ID, packetHandler->getRxPacketError(dxl_error)); + } + else if (groupBulkRead.getError(DXL2_ID, &dxl_error)) + { + printf("[ID:%03d] %s\n", DXL2_ID, packetHandler->getRxPacketError(dxl_error)); + } dxl_getdata_result = groupBulkRead.isAvailable(DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION); if (dxl_getdata_result != true) diff --git a/c++/example/protocol1.0/reset/reset.cpp b/c++/example/protocol1.0/reset/reset.cpp index 1672b164..d29e2b41 100644 --- a/c++/example/protocol1.0/reset/reset.cpp +++ b/c++/example/protocol1.0/reset/reset.cpp @@ -32,6 +32,7 @@ #if defined(__linux__) || defined(__APPLE__) #include #include +#include #define STDIN_FILENO 0 #elif defined(_WIN32) || defined(_WIN64) #include diff --git a/c++/example/protocol2.0/bulk_read_write/bulk_read_write.cpp b/c++/example/protocol2.0/bulk_read_write/bulk_read_write.cpp index 053cde1f..fbf87e91 100644 --- a/c++/example/protocol2.0/bulk_read_write/bulk_read_write.cpp +++ b/c++/example/protocol2.0/bulk_read_write/bulk_read_write.cpp @@ -256,7 +256,18 @@ int main() { // Bulkread present position and LED status dxl_comm_result = groupBulkRead.txRxPacket(); - if (dxl_comm_result != COMM_SUCCESS) printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (groupBulkRead.getError(DXL1_ID, &dxl_error)) + { + printf("[ID:%03d] %s\n", DXL1_ID, packetHandler->getRxPacketError(dxl_error)); + } + else if (groupBulkRead.getError(DXL2_ID, &dxl_error)) + { + printf("[ID:%03d] %s\n", DXL2_ID, packetHandler->getRxPacketError(dxl_error)); + } // Check if groupbulkread data of Dynamixel#1 is available dxl_getdata_result = groupBulkRead.isAvailable(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); diff --git a/c++/example/protocol2.0/factory_reset/factory_reset.cpp b/c++/example/protocol2.0/factory_reset/factory_reset.cpp index 2621be15..4fcfdd18 100644 --- a/c++/example/protocol2.0/factory_reset/factory_reset.cpp +++ b/c++/example/protocol2.0/factory_reset/factory_reset.cpp @@ -32,6 +32,7 @@ #if defined(__linux__) || defined(__APPLE__) #include #include +#include #define STDIN_FILENO 0 #elif defined(_WIN32) || defined(_WIN64) #include diff --git a/c++/example/protocol2.0/sync_read_write/sync_read_write.cpp b/c++/example/protocol2.0/sync_read_write/sync_read_write.cpp index 978d141a..0d370b5d 100644 --- a/c++/example/protocol2.0/sync_read_write/sync_read_write.cpp +++ b/c++/example/protocol2.0/sync_read_write/sync_read_write.cpp @@ -252,7 +252,18 @@ int main() { // Syncread present position dxl_comm_result = groupSyncRead.txRxPacket(); - if (dxl_comm_result != COMM_SUCCESS) printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + if (dxl_comm_result != COMM_SUCCESS) + { + printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (groupSyncRead.getError(DXL1_ID, &dxl_error)) + { + printf("[ID:%03d] %s\n", DXL1_ID, packetHandler->getRxPacketError(dxl_error)); + } + else if (groupSyncRead.getError(DXL2_ID, &dxl_error)) + { + printf("[ID:%03d] %s\n", DXL2_ID, packetHandler->getRxPacketError(dxl_error)); + } // Check if groupsyncread data of Dynamixel#1 is available dxl_getdata_result = groupSyncRead.isAvailable(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); diff --git a/c++/include/dynamixel_sdk/group_bulk_read.h b/c++/include/dynamixel_sdk/group_bulk_read.h index 3dec1aa9..624c00de 100644 --- a/c++/include/dynamixel_sdk/group_bulk_read.h +++ b/c++/include/dynamixel_sdk/group_bulk_read.h @@ -44,6 +44,7 @@ class WINDECLSPEC GroupBulkRead std::map address_list_; // std::map length_list_; // std::map data_list_; // + std::map error_list_; // bool last_result_; bool is_param_changed_; @@ -148,6 +149,16 @@ class WINDECLSPEC GroupBulkRead /// @return data value //////////////////////////////////////////////////////////////////////////////// uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets the error which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket + /// @param id Dynamixel ID + /// @error error of Dynamixel + /// @return true + /// @return when Dynamixel returned specific error byte + /// @return or false + //////////////////////////////////////////////////////////////////////////////// + bool getError (uint8_t id, uint8_t* error); }; } diff --git a/c++/include/dynamixel_sdk/group_sync_read.h b/c++/include/dynamixel_sdk/group_sync_read.h index ae6d82e7..0b934f60 100644 --- a/c++/include/dynamixel_sdk/group_sync_read.h +++ b/c++/include/dynamixel_sdk/group_sync_read.h @@ -41,7 +41,8 @@ class WINDECLSPEC GroupSyncRead PacketHandler *ph_; std::vector id_list_; - std::map data_list_; // + std::map data_list_; // + std::map error_list_; // bool last_result_; bool is_param_changed_; @@ -152,6 +153,16 @@ class WINDECLSPEC GroupSyncRead /// @return data value //////////////////////////////////////////////////////////////////////////////// uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets the error which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket + /// @param id Dynamixel ID + /// @error error of Dynamixel + /// @return true + /// @return when Dynamixel returned specific error byte + /// @return or false + //////////////////////////////////////////////////////////////////////////////// + bool getError (uint8_t id, uint8_t* error); }; } diff --git a/c++/include/dynamixel_sdk/packet_handler.h b/c++/include/dynamixel_sdk/packet_handler.h index 439ada58..37ff2400 100644 --- a/c++/include/dynamixel_sdk/packet_handler.h +++ b/c++/include/dynamixel_sdk/packet_handler.h @@ -100,13 +100,6 @@ class WINDECLSPEC PacketHandler //////////////////////////////////////////////////////////////////////////////// virtual const char *getTxRxResult (int result) = 0; - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that prints out description of communication result - /// @param result Communication result which might be gotten by the tx rx functions - /// @todo This function is deprecated (removed in DynamixelSDK ver. 3.6.1) - //////////////////////////////////////////////////////////////////////////////// - DEPRECATED virtual void printTxRxResult (int result) = 0; - //////////////////////////////////////////////////////////////////////////////// /// @brief The function that gets description of hardware error /// @param error Dynamixel hardware error which might be gotten by the tx rx functions @@ -114,13 +107,6 @@ class WINDECLSPEC PacketHandler //////////////////////////////////////////////////////////////////////////////// virtual const char *getRxPacketError (uint8_t error) = 0; - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that prints out description of hardware error - /// @param error Dynamixel hardware error which might be gotten by the tx rx functions - /// @todo This function is deprecated (removed in DynamixelSDK ver. 3.6.1) - //////////////////////////////////////////////////////////////////////////////// - DEPRECATED virtual void printRxPacketError (uint8_t error) = 0; - //////////////////////////////////////////////////////////////////////////////// /// @brief The function that transmits the instruction packet txpacket via PortHandler port. /// @description The function clears the port buffer by PortHandler::clearPort() function, diff --git a/c++/include/dynamixel_sdk/protocol1_packet_handler.h b/c++/include/dynamixel_sdk/protocol1_packet_handler.h index 4347c788..ebfa05c0 100644 --- a/c++/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/c++/include/dynamixel_sdk/protocol1_packet_handler.h @@ -60,13 +60,6 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// const char *getTxRxResult (int result); - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that prints out description of communication result - /// @param result Communication result which might be gotten by the tx rx functions - /// @todo This function is deprecated (removed in DynamixelSDK ver. 3.6.1) - //////////////////////////////////////////////////////////////////////////////// - DEPRECATED void printTxRxResult (int result); - //////////////////////////////////////////////////////////////////////////////// /// @brief The function that gets description of hardware error /// @param error Dynamixel hardware error which might be gotten by the tx rx functions @@ -74,13 +67,6 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// const char *getRxPacketError (uint8_t error); - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that prints out description of hardware error - /// @param error Dynamixel hardware error which might be gotten by the tx rx functions - /// @todo This function is deprecated (removed in DynamixelSDK ver. 3.6.1) - //////////////////////////////////////////////////////////////////////////////// - DEPRECATED void printRxPacketError (uint8_t error); - //////////////////////////////////////////////////////////////////////////////// /// @brief The function that transmits the instruction packet txpacket via PortHandler port. /// @description The function clears the port buffer by PortHandler::clearPort() function, diff --git a/c++/include/dynamixel_sdk/protocol2_packet_handler.h b/c++/include/dynamixel_sdk/protocol2_packet_handler.h index ff369323..3c953f39 100644 --- a/c++/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/c++/include/dynamixel_sdk/protocol2_packet_handler.h @@ -64,13 +64,6 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// const char *getTxRxResult (int result); - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that prints out description of communication result - /// @param result Communication result which might be gotten by the tx rx functions - /// @todo This function is deprecated (removed in DynamixelSDK ver. 3.6.1) - //////////////////////////////////////////////////////////////////////////////// - DEPRECATED void printTxRxResult (int result); - //////////////////////////////////////////////////////////////////////////////// /// @brief The function that gets description of hardware error /// @param error Dynamixel hardware error which might be gotten by the tx rx functions @@ -78,13 +71,6 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler //////////////////////////////////////////////////////////////////////////////// const char *getRxPacketError (uint8_t error); - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that prints out description of hardware error - /// @param error Dynamixel hardware error which might be gotten by the tx rx functions - /// @todo This function is deprecated (removed in DynamixelSDK ver. 3.6.1) - //////////////////////////////////////////////////////////////////////////////// - DEPRECATED void printRxPacketError (uint8_t error); - //////////////////////////////////////////////////////////////////////////////// /// @brief The function that transmits the instruction packet txpacket via PortHandler port. /// @description The function clears the port buffer by PortHandler::clearPort() function, diff --git a/c++/library.properties b/c++/library.properties index 61c03a03..b7b64bcc 100644 --- a/c++/library.properties +++ b/c++/library.properties @@ -1,5 +1,5 @@ name=DynamixelSDK -version=3.5.4 +version=3.6.0 author=ROBOTIS maintainer=ROBOTIS sentence=DynamixelSDK for Arduino diff --git a/c++/package.xml b/c++/package.xml index 8a74e8df..a31b8eb1 100644 --- a/c++/package.xml +++ b/c++/package.xml @@ -1,7 +1,7 @@ dynamixel_sdk - 3.5.4 + 3.6.0 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. Apache-2.0 Zerom diff --git a/c++/src/dynamixel_sdk/group_bulk_read.cpp b/c++/src/dynamixel_sdk/group_bulk_read.cpp index dfd73b62..bcea7551 100644 --- a/c++/src/dynamixel_sdk/group_bulk_read.cpp +++ b/c++/src/dynamixel_sdk/group_bulk_read.cpp @@ -90,6 +90,7 @@ bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_l length_list_[id] = data_length; address_list_[id] = start_address; data_list_[id] = new uint8_t[data_length]; + error_list_[id] = new uint8_t[1]; is_param_changed_ = true; return true; @@ -105,7 +106,9 @@ void GroupBulkRead::removeParam(uint8_t id) address_list_.erase(id); length_list_.erase(id); delete[] data_list_[id]; + delete[] error_list_[id]; data_list_.erase(id); + error_list_.erase(id); is_param_changed_ = true; } @@ -116,12 +119,16 @@ void GroupBulkRead::clearParam() return; for (unsigned int i = 0; i < id_list_.size(); i++) + { delete[] data_list_[id_list_[i]]; + delete[] error_list_[id_list_[i]]; + } id_list_.clear(); address_list_.clear(); length_list_.clear(); data_list_.clear(); + error_list_.clear(); if (param_ != 0) delete[] param_; param_ = 0; @@ -159,7 +166,7 @@ int GroupBulkRead::rxPacket() { uint8_t id = id_list_[i]; - result = ph_->readRx(port_, id, length_list_[id], data_list_[id]); + result = ph_->readRx(port_, id, length_list_[id], data_list_[id], error_list_[id]); if (result != COMM_SUCCESS) return result; } @@ -219,3 +226,20 @@ uint32_t GroupBulkRead::getData(uint8_t id, uint16_t address, uint16_t data_leng return 0; } } + +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; + } +} \ 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 1709dc7b..f392943b 100644 --- a/c++/src/dynamixel_sdk/group_sync_read.cpp +++ b/c++/src/dynamixel_sdk/group_sync_read.cpp @@ -69,6 +69,7 @@ bool GroupSyncRead::addParam(uint8_t id) id_list_.push_back(id); data_list_[id] = new uint8_t[data_length_]; + error_list_[id] = new uint8_t[1]; is_param_changed_ = true; return true; @@ -84,7 +85,9 @@ void GroupSyncRead::removeParam(uint8_t id) id_list_.erase(it); delete[] data_list_[id]; + delete[] error_list_[id]; data_list_.erase(id); + error_list_.erase(id); is_param_changed_ = true; } @@ -94,10 +97,14 @@ void GroupSyncRead::clearParam() return; for (unsigned int i = 0; i < id_list_.size(); i++) + { delete[] data_list_[id_list_[i]]; + delete[] error_list_[id_list_[i]]; + } id_list_.clear(); data_list_.clear(); + error_list_.clear(); if (param_ != 0) delete[] param_; param_ = 0; @@ -131,7 +138,7 @@ int GroupSyncRead::rxPacket() { uint8_t id = id_list_[i]; - result = ph_->readRx(port_, id, data_length_, data_list_[id]); + result = ph_->readRx(port_, id, data_length_, data_list_[id], error_list_[id]); if (result != COMM_SUCCESS) return result; } @@ -188,3 +195,20 @@ uint32_t GroupSyncRead::getData(uint8_t id, uint16_t address, uint16_t data_leng return 0; } } + +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; + } +} \ No newline at end of file diff --git a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp index a3559359..f8efe111 100644 --- a/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -93,17 +93,6 @@ const char *Protocol1PacketHandler::getTxRxResult(int result) } } -void Protocol1PacketHandler::printTxRxResult(int result) -{ -#if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) - Serial.println("This function is deprecated. Use 'Serial.print()' and 'getRxPacketError()' instead"); - Serial.println(getTxRxResult(result)); -#else - printf("This function is deprecated. Use 'printf()' and 'getRxPacketError()' instead\n"); - printf("%s\n", getTxRxResult(result)); -#endif -} - const char *Protocol1PacketHandler::getRxPacketError(uint8_t error) { if (error & ERRBIT_VOLTAGE) @@ -130,17 +119,6 @@ const char *Protocol1PacketHandler::getRxPacketError(uint8_t error) return ""; } -void Protocol1PacketHandler::printRxPacketError(uint8_t error) -{ -#if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) - Serial.println("This function is deprecated. Use 'Serial.print()' and 'getRxPacketError()' instead"); - Serial.println(getRxPacketError(error)); -#else - printf("This function is deprecated. Use 'printf()' and 'getRxPacketError()' instead\n"); - printf("%s\n", getRxPacketError(error)); -#endif -} - int Protocol1PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) { uint8_t checksum = 0; diff --git a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp index 2b35069c..c897e620 100644 --- a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -99,17 +99,6 @@ const char *Protocol2PacketHandler::getTxRxResult(int result) } } -void Protocol2PacketHandler::printTxRxResult(int result) -{ -#if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) - Serial.println("This function is deprecated. Use 'Serial.print()' and 'getRxPacketError()' instead"); - Serial.println(getTxRxResult(result)); -#else - printf("This function is deprecated. Use 'printf()' and 'getRxPacketError()' instead\n"); - printf("%s\n", getTxRxResult(result)); -#endif -} - const char *Protocol2PacketHandler::getRxPacketError(uint8_t error) { if (error & ERRBIT_ALERT) @@ -148,17 +137,6 @@ const char *Protocol2PacketHandler::getRxPacketError(uint8_t error) } } -void Protocol2PacketHandler::printRxPacketError(uint8_t error) -{ -#if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) - Serial.println("This function is deprecated. Use 'Serial.print()' and 'getRxPacketError()' instead"); - Serial.println(getRxPacketError(error)); -#else - printf("This function is deprecated. Use 'printf()' and 'getRxPacketError()' instead\n"); - printf("%s\n", getRxPacketError(error)); -#endif -} - unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) { uint16_t i; @@ -711,7 +689,7 @@ int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t add { if (error != 0) *error = (uint8_t)rxpacket[PKT_ERROR]; - + for (uint16_t s = 0; s < length; s++) { data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; @@ -914,7 +892,7 @@ int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - for (uint8_t s = 0; s < length; s++) + for (uint16_t s = 0; s < length; s++) txpacket[PKT_PARAMETER0+2+s] = data[s]; //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); diff --git a/c/example/protocol1.0/reset/reset.c b/c/example/protocol1.0/reset/reset.c index f2d2ba6e..8dea7f6f 100644 --- a/c/example/protocol1.0/reset/reset.c +++ b/c/example/protocol1.0/reset/reset.c @@ -33,6 +33,7 @@ #if defined(__linux__) || defined(__APPLE__) #include #include +#include #define STDIN_FILENO 0 #elif defined(_WIN32) || defined(_WIN64) #include diff --git a/c/example/protocol2.0/factory_reset/factory_reset.c b/c/example/protocol2.0/factory_reset/factory_reset.c index d08d2380..f7d51c3e 100644 --- a/c/example/protocol2.0/factory_reset/factory_reset.c +++ b/c/example/protocol2.0/factory_reset/factory_reset.c @@ -33,6 +33,7 @@ #if defined(__linux__) || defined(__APPLE__) #include #include +#include #define STDIN_FILENO 0 #elif defined(_WIN32) || defined(_WIN64) #include diff --git a/c/include/dynamixel_sdk/packet_handler.h b/c/include/dynamixel_sdk/packet_handler.h index be381e6a..416d69f9 100644 --- a/c/include/dynamixel_sdk/packet_handler.h +++ b/c/include/dynamixel_sdk/packet_handler.h @@ -78,9 +78,7 @@ PacketData *packetData; WINDECLSPEC void packetHandler (); WINDECLSPEC const char *getTxRxResult (int protocol_version, int result); -DEPRECATED WINDECLSPEC void printTxRxResult (int protocol_version, int result); WINDECLSPEC const char *getRxPacketError (int protocol_version, uint8_t error); -DEPRECATED WINDECLSPEC void printRxPacketError (int protocol_version, uint8_t error); WINDECLSPEC int getLastTxRxResult (int port_num, int protocol_version); WINDECLSPEC uint8_t getLastRxPacketError (int port_num, int protocol_version); diff --git a/c/include/dynamixel_sdk/protocol1_packet_handler.h b/c/include/dynamixel_sdk/protocol1_packet_handler.h index 298480df..b9de05bc 100644 --- a/c/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/c/include/dynamixel_sdk/protocol1_packet_handler.h @@ -23,9 +23,7 @@ #include "packet_handler.h" WINDECLSPEC const char *getTxRxResult1 (int result); -DEPRECATED WINDECLSPEC void printTxRxResult1 (int result); WINDECLSPEC const char *getRxPacketError1 (uint8_t error); -DEPRECATED WINDECLSPEC void printRxPacketError1 (uint8_t error); WINDECLSPEC int getLastTxRxResult1 (int port_num); WINDECLSPEC uint8_t getLastRxPacketError1 (int port_num); diff --git a/c/include/dynamixel_sdk/protocol2_packet_handler.h b/c/include/dynamixel_sdk/protocol2_packet_handler.h index 65232251..6cacd8f0 100644 --- a/c/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/c/include/dynamixel_sdk/protocol2_packet_handler.h @@ -27,9 +27,7 @@ WINDECLSPEC void addStuffing (uint8_t *packet); WINDECLSPEC void removeStuffing (uint8_t *packet); WINDECLSPEC const char *getTxRxResult2 (int result); -DEPRECATED WINDECLSPEC void printTxRxResult2 (int result); WINDECLSPEC const char *getRxPacketError2 (uint8_t error); -DEPRECATED WINDECLSPEC void printRxPacketError2 (uint8_t error); WINDECLSPEC int getLastTxRxResult2 (int port_num); WINDECLSPEC uint8_t getLastRxPacketError2 (int port_num); diff --git a/c/include/dynamixel_sdk/robotis_def.h b/c/include/dynamixel_sdk/robotis_def.h index ccc556dd..35fdbdd6 100644 --- a/c/include/dynamixel_sdk/robotis_def.h +++ b/c/include/dynamixel_sdk/robotis_def.h @@ -20,9 +20,9 @@ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_C_H_ #if defined(_WIN32) || defined(_WIN64) -typedef char int8_t; -typedef short int int16_t; -typedef int int32_t; +typedef signed char int8_t; +typedef signed short int int16_t; +typedef signed int int32_t; #endif typedef unsigned char uint8_t; diff --git a/c/src/dynamixel_sdk/protocol1_packet_handler.c b/c/src/dynamixel_sdk/protocol1_packet_handler.c index a7ed5cbe..1e492a9c 100644 --- a/c/src/dynamixel_sdk/protocol1_packet_handler.c +++ b/c/src/dynamixel_sdk/protocol1_packet_handler.c @@ -85,12 +85,6 @@ const char *getTxRxResult1(int result) } } -void printTxRxResult1(int result) -{ - printf("This function is deprecated. Use getTxRxResult instead\n"); - printf("%s\n", getTxRxResult1(result)); -} - const char *getRxPacketError1(uint8_t error) { if (error & ERRBIT_VOLTAGE) @@ -117,12 +111,6 @@ const char *getRxPacketError1(uint8_t error) return ""; } -void printRxPacketError1(uint8_t error) -{ - printf("This function is deprecated. Use getRxPacketError instead\n"); - printf("%s\n", getRxPacketError1(error)); -} - int getLastTxRxResult1(int port_num) { return packetData[port_num].communication_result; diff --git a/c/src/dynamixel_sdk/protocol2_packet_handler.c b/c/src/dynamixel_sdk/protocol2_packet_handler.c index 25350571..c0f475da 100644 --- a/c/src/dynamixel_sdk/protocol2_packet_handler.c +++ b/c/src/dynamixel_sdk/protocol2_packet_handler.c @@ -91,12 +91,6 @@ const char *getTxRxResult2(int result) } } -void printTxRxResult2(int result) -{ - printf("This function is deprecated. Use getTxRxResult instead\n"); - printf("%s\n", getTxRxResult2(result)); -} - const char *getRxPacketError2(uint8_t error) { int not_alert_error; @@ -136,12 +130,6 @@ const char *getRxPacketError2(uint8_t error) } } -void printRxPacketError2(uint8_t error) -{ - printf("This function is deprecated. Use getRxPacketError instead\n"); - printf("%s\n", getRxPacketError2(error)); -} - int getLastTxRxResult2(int port_num) { return packetData[port_num].communication_result; diff --git a/java/dynamixel_functions_java/mac_x64/Dynamixel.java b/java/dynamixel_functions_java/mac_x64/Dynamixel.java index e5c42e36..1ef0d192 100644 --- a/java/dynamixel_functions_java/mac_x64/Dynamixel.java +++ b/java/dynamixel_functions_java/mac_x64/Dynamixel.java @@ -44,9 +44,7 @@ interface LibFunction extends Library // PacketHandler functions public void packetHandler (); - public void printTxRxResult (int protocol_version, int result); public String getTxRxResult (int protocol_version, int result); - public void printRxPacketError (int protocol_version, byte error); public String getRxPacketError (int protocol_version, byte error); public int getLastTxRxResult (int port_num, int protocol_version); diff --git a/java/dynamixel_functions_java/x64/Dynamixel.java b/java/dynamixel_functions_java/x64/Dynamixel.java index 4c306ea7..ee2d2259 100644 --- a/java/dynamixel_functions_java/x64/Dynamixel.java +++ b/java/dynamixel_functions_java/x64/Dynamixel.java @@ -44,9 +44,7 @@ interface LibFunction extends Library // PacketHandler functions public void packetHandler (); - public void printTxRxResult (int protocol_version, int result); public String getTxRxResult (int protocol_version, int result); - public void printRxPacketError (int protocol_version, byte error); public String getRxPacketError (int protocol_version, byte error); public int getLastTxRxResult (int port_num, int protocol_version); diff --git a/java/dynamixel_functions_java/x86/Dynamixel.java b/java/dynamixel_functions_java/x86/Dynamixel.java index 7dda4a47..8760d4fe 100644 --- a/java/dynamixel_functions_java/x86/Dynamixel.java +++ b/java/dynamixel_functions_java/x86/Dynamixel.java @@ -44,9 +44,7 @@ interface LibFunction extends Library // PacketHandler functions public void packetHandler (); - public void printTxRxResult (int protocol_version, int result); public String getTxRxResult (int protocol_version, int result); - public void printRxPacketError (int protocol_version, byte error); public String getRxPacketError (int protocol_version, byte error); public int getLastTxRxResult (int port_num, int protocol_version); diff --git a/matlab/m_basic_function/packet_handler/printRxPacketError.m b/matlab/m_basic_function/packet_handler/printRxPacketError.m deleted file mode 100644 index de934f04..00000000 --- a/matlab/m_basic_function/packet_handler/printRxPacketError.m +++ /dev/null @@ -1,36 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 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) - -function [] = printRxPacketError( protocol_version, error ) - -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, 'printRxPacketError', protocol_version, error); -end diff --git a/matlab/m_basic_function/packet_handler/printTxRxResult.m b/matlab/m_basic_function/packet_handler/printTxRxResult.m deleted file mode 100644 index e72556b5..00000000 --- a/matlab/m_basic_function/packet_handler/printTxRxResult.m +++ /dev/null @@ -1,36 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 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) - -function [] = printTxRxResult( protocol_version, result ) - -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, 'printTxRxResult', protocol_version, result); -end diff --git a/python/LICENSE.txt b/python/LICENSE.txt new file mode 100644 index 00000000..9c8f3ea0 --- /dev/null +++ b/python/LICENSE.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + 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. \ No newline at end of file diff --git a/python/README.txt b/python/README.txt new file mode 100644 index 00000000..71473146 --- /dev/null +++ b/python/README.txt @@ -0,0 +1 @@ +Description is available at http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/ \ No newline at end of file diff --git a/python/dynamixel_functions_py/dynamixel_functions.py b/python/dynamixel_functions_py/dynamixel_functions.py deleted file mode 100644 index 7518ccf9..00000000 --- a/python/dynamixel_functions_py/dynamixel_functions.py +++ /dev/null @@ -1,173 +0,0 @@ -#!/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) - -import ctypes -from ctypes import cdll -dxl_lib = cdll.LoadLibrary("../../c/build/win32/output/dxl_x86_c.dll") # for windows 32bit -# dxl_lib = cdll.LoadLibrary("../../c/build/win64/output/dxl_x64_c.dll") # for windows 64bit -# dxl_lib = cdll.LoadLibrary("../../c/build/linux32/libdxl_x86_c.so") # for linux 32bit -# dxl_lib = cdll.LoadLibrary("../../c/build/linux64/libdxl_x64_c.so") # for linux 64bit -# dxl_lib = cdll.LoadLibrary("../../c/build/linux_sbc/libdxl_sbc_c.so") # for SBC linux -# dxl_lib = cdll.LoadLibrary("../../c/build/mac/libdxl_mac_c.dylib") # for Mac OS - -# port_handler -portHandler = dxl_lib.portHandler - -openPort = dxl_lib.openPort -closePort = dxl_lib.closePort -clearPort = dxl_lib.clearPort - -setPortName = dxl_lib.setPortName -getPortName = dxl_lib.getPortName - -setBaudRate = dxl_lib.setBaudRate -getBaudRate = dxl_lib.getBaudRate - -readPort = dxl_lib.readPort -writePort = dxl_lib.writePort - -setPacketTimeout = dxl_lib.setPacketTimeout -setPacketTimeoutMSec = dxl_lib.setPacketTimeoutMSec -isPacketTimeout = dxl_lib.isPacketTimeout - -# packet_handler -packetHandler = dxl_lib.packetHandler - -printTxRxResult = dxl_lib.printTxRxResult -getTxRxResult = dxl_lib.getTxRxResult -getTxRxResult.restype = ctypes.c_char_p -printRxPacketError = dxl_lib.printRxPacketError -getRxPacketError = dxl_lib.getRxPacketError -getRxPacketError.restype = ctypes.c_char_p - -getLastTxRxResult = dxl_lib.getLastTxRxResult -getLastRxPacketError = dxl_lib.getLastRxPacketError - -setDataWrite = dxl_lib.setDataWrite -getDataRead = dxl_lib.getDataRead - -txPacket = dxl_lib.txPacket - -rxPacket = dxl_lib.rxPacket - -txRxPacket = dxl_lib.txRxPacket - -ping = dxl_lib.ping - -pingGetModelNum = dxl_lib.pingGetModelNum - -broadcastPing = dxl_lib.broadcastPing -getBroadcastPingResult = dxl_lib.getBroadcastPingResult - -reboot = dxl_lib.reboot - -factoryReset = dxl_lib.factoryReset - -readTx = dxl_lib.readTx -readRx = dxl_lib.readRx -readTxRx = dxl_lib.readTxRx - -read1ByteTx = dxl_lib.read1ByteTx -read1ByteRx = dxl_lib.read1ByteRx -read1ByteTxRx = dxl_lib.read1ByteTxRx - -read2ByteTx = dxl_lib.read2ByteTx -read2ByteRx = dxl_lib.read2ByteRx -read2ByteTxRx = dxl_lib.read2ByteTxRx - -read4ByteTx = dxl_lib.read4ByteTx -read4ByteRx = dxl_lib.read4ByteRx -read4ByteTxRx = dxl_lib.read4ByteTxRx - -writeTxOnly = dxl_lib.writeTxOnly -writeTxRx = dxl_lib.writeTxRx - -write1ByteTxOnly = dxl_lib.write1ByteTxOnly -write1ByteTxRx = dxl_lib.write1ByteTxRx - -write2ByteTxOnly = dxl_lib.write2ByteTxOnly -write2ByteTxRx = dxl_lib.write2ByteTxRx - -write4ByteTxOnly = dxl_lib.write4ByteTxOnly -write4ByteTxRx = dxl_lib.write4ByteTxRx - -regWriteTxOnly = dxl_lib.regWriteTxOnly -regWriteTxRx = dxl_lib.regWriteTxRx - -syncReadTx = dxl_lib.syncReadTx -# syncReadRx -> GroupSyncRead -# syncReadTxRx -> GroupSyncRead - -syncWriteTxOnly = dxl_lib.syncWriteTxOnly - -bulkReadTx = dxl_lib.bulkReadTx -# bulkReadRx -> GroupBulkRead -# bulkReadTxRx -> GroupBulkRead - -bulkWriteTxOnly = dxl_lib.bulkWriteTxOnly - -# group_bulk_read -groupBulkRead = dxl_lib.groupBulkRead - -groupBulkReadAddParam = dxl_lib.groupBulkReadAddParam -groupBulkReadRemoveParam = dxl_lib.groupBulkReadRemoveParam -groupBulkReadClearParam = dxl_lib.groupBulkReadClearParam - -groupBulkReadTxPacket = dxl_lib.groupBulkReadTxPacket -groupBulkReadRxPacket = dxl_lib.groupBulkReadRxPacket -groupBulkReadTxRxPacket = dxl_lib.groupBulkReadTxRxPacket - -groupBulkReadIsAvailable = dxl_lib.groupBulkReadIsAvailable -groupBulkReadGetData = dxl_lib.groupBulkReadGetData - -#group_bulk_write -groupBulkWrite = dxl_lib.groupBulkWrite - -groupBulkWriteAddParam = dxl_lib.groupBulkWriteAddParam -groupBulkWriteRemoveParam = dxl_lib.groupBulkWriteRemoveParam -groupBulkWriteChangeParam = dxl_lib.groupBulkWriteChangeParam -groupBulkWriteClearParam = dxl_lib.groupBulkWriteClearParam - -groupBulkWriteTxPacket = dxl_lib.groupBulkWriteTxPacket - -#group_sync_read -groupSyncRead = dxl_lib.groupSyncRead - -groupSyncReadAddParam = dxl_lib.groupSyncReadAddParam -groupSyncReadRemoveParam = dxl_lib.groupSyncReadRemoveParam -groupSyncReadClearParam = dxl_lib.groupSyncReadClearParam - -groupSyncReadTxPacket = dxl_lib.groupSyncReadTxPacket -groupSyncReadRxPacket = dxl_lib.groupSyncReadRxPacket -groupSyncReadTxRxPacket = dxl_lib.groupSyncReadTxRxPacket - -groupSyncReadIsAvailable = dxl_lib.groupSyncReadIsAvailable -groupSyncReadGetData = dxl_lib.groupSyncReadGetData - -#group_sync_write -groupSyncWrite = dxl_lib.groupSyncWrite - -groupSyncWriteAddParam = dxl_lib.groupSyncWriteAddParam -groupSyncWriteRemoveParam = dxl_lib.groupSyncWriteRemoveParam -groupSyncWriteChangeParam = dxl_lib.groupSyncWriteChangeParam -groupSyncWriteClearParam = dxl_lib.groupSyncWriteClearParam - -groupSyncWriteTxPacket = dxl_lib.groupSyncWriteTxPacket diff --git a/python/protocol1_0/bulk_read.py b/python/protocol1_0/bulk_read.py deleted file mode 100644 index 765a8567..00000000 --- a/python/protocol1_0/bulk_read.py +++ /dev/null @@ -1,241 +0,0 @@ -#!/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) - -# -# ********* Bulk Read Example ********* -# -# -# Available Dynamixel model on this example : MX or X series set to Protocol 1.0 -# This example is designed for using two Dynamixel MX-28, and an USB2DYNAMIXEL. -# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) -# - -import os, ctypes - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_MX_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model -ADDR_MX_GOAL_POSITION = 30 -ADDR_MX_PRESENT_POSITION = 36 -ADDR_MX_MOVING = 46 - -# Data Byte Length -LEN_MX_GOAL_POSITION = 2 -LEN_MX_PRESENT_POSITION = 2 -LEN_MX_MOVING = 1 - -# Protocol version -PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel - -# Default setting -DXL1_ID = 1 # Dynamixel ID: 1 -DXL2_ID = 2 # Dynamixel ID: 2 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# 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 -DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 10 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -# Initialize Groupsyncwrite instance -group_num = dynamixel.groupBulkRead(port_num, PROTOCOL_VERSION) - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_addparam_result = 0 # AddParam result -dxl_getdata_result = 0 # GetParam result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl1_present_position = 0 # Present position -dxl2_moving = 0 # Dynamixel moving status - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#1 has been successfully connected") - -# Enable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#2 has been successfully connected") - -# Add parameter storage for Dynamixel#1 present position value -dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkReadAddParam(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION)).value -if dxl_addparam_result != 1: - print("[ID:%03d] groupBulkRead addparam failed" % (DXL1_ID)) - quit() - -# Add parameter storage for Dynamixel#2 present moving value -dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkReadAddParam(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING)).value -if dxl_addparam_result != 1: - print("[ID:%03d] groupBulkRead addparam failed" % (DXL2_ID)) - quit() - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Write Dynamixel#1 goal position - dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - # Write Dynamixel#2 goal position - dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - while 1: - # Bulkread present position and moving status - dynamixel.groupBulkReadTxRxPacket(group_num) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - - # Check if groupbulkread data of Dynamixel#1 is available - dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION)).value - if dxl_getdata_result != 1: - print("[ID:%03d] groupBulkRead getdata failed" % (DXL1_ID)) - quit() - - # Check if groupbulkread data of Dynamixel#2 is available - dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING)).value - if dxl_getdata_result != 1: - print("[ID:%03d] groupBulkRead getdata failed" % (DXL2_ID)) - quit() - - # Get Dynamixel#1 present position value - dxl1_present_position = dynamixel.groupBulkReadGetData(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION) - - # Get Dynamixel#2 moving status value - dxl2_moving = dynamixel.groupBulkReadGetData(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING) - - print("[ID:%03d] Present Position : %d \t [ID:%03d] Is Moving : %d" % (DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_moving)) - - if not (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Disable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol1_0/factory_reset.py b/python/protocol1_0/factory_reset.py deleted file mode 100644 index 986ca175..00000000 --- a/python/protocol1_0/factory_reset.py +++ /dev/null @@ -1,185 +0,0 @@ -#!/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) - -# -# ********* Factory Reset Example ********* -# -# -# Available Dynamixel model on this example : All models using Protocol 1.0 -# This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL. -# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) -# - -# Be aware that: -# This example resets all properties of Dynamixel to default values, such as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) -# - -import os - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -from time import sleep -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_MX_BAUDRATE = 4 # Control table address is different in Dynamixel model - -# Protocol version -PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel - -# Default setting -DXL_ID = 1 # Dynamixel ID: 1 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode("utf-8")# Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" - -FACTORYRST_DEFAULTBAUDRATE = 57600 # Dynamixel baudrate set by factoryreset -NEW_BAUDNUM = 1 # New baudnum to recover Dynamixel baudrate as it was -OPERATION_MODE = 0x00 # Mode is unavailable in Protocol 1.0 Reset - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -dxl_comm_result = COMM_TX_FAIL # Communication result - -dxl_error = 0 # Dynamixel error -dxl_baudnum_read = 0 # Read baudnum - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Read present baudrate of the controller -print("Now the controller baudrate is : %d" % (dynamixel.getBaudRate(port_num))) - -# Try factoryreset -print("[ID:%03d] Try factoryreset : " % (DXL_ID)) -dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print("Aborted") - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - -# Wait for reset -print("Wait for reset...") -sleep(2) - -print("[ID:%03d] factoryReset Success!" % (DXL_ID)) - -# Set controller baudrate to dxl default baudrate -if dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE): - print("Succeed to change the controller baudrate to : %d" % (FACTORYRST_DEFAULTBAUDRATE)) -else: - print("Failed to change the controller baudrate") - getch() - quit() - -# Read Dynamixel baudnum -dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) - -# Write new baudnum -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE, NEW_BAUDNUM) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM)) - -# Set port baudrate to BAUDRATE -if dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeed to change the controller baudrate to : %d" % (BAUDRATE)) -else: - print("Failed to change the controller baudrate") - getch() - quit() - -sleep(0.2) - -# Read Dynamixel baudnum -dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) - - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol1_0/multi_port.py b/python/protocol1_0/multi_port.py deleted file mode 100644 index caa11ff1..00000000 --- a/python/protocol1_0/multi_port.py +++ /dev/null @@ -1,233 +0,0 @@ -#!/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) - -# -# ********* Multi Port Example ********* -# -# -# Available Dynamixel model on this example : All models using Protocol 1.0 -# This example is designed for using two Dynamixel MX-28, and two USB2DYNAMIXEL. -# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) -# - -import os - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_MX_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model -ADDR_MX_GOAL_POSITION = 30 -ADDR_MX_PRESENT_POSITION = 36 - -# Protocol version -PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel - -# Default setting -DXL1_ID = 1 # Dynamixel ID: 1 -DXL2_ID = 2 # Dynamixel ID: 2 -BAUDRATE = 57600 -DEVICENAME1 = "/dev/ttyUSB0".encode('utf-8')# Check which port is being used on your controller -DEVICENAME2 = "/dev/ttyUSB1".encode('utf-8')# 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 -DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 10 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -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_num1 = dynamixel.portHandler(DEVICENAME1) -port_num2 = dynamixel.portHandler(DEVICENAME2) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl1_present_position = 0 # Present position -dxl2_present_position = 0 - -# Open port1 -if dynamixel.openPort(port_num1): - print("Succeeded to open the port!") -else: - print("Failed to open the port!") - print("Press any key to terminate...") - getch() - quit() - -# Open port2 -if dynamixel.openPort(port_num2): - print("Succeeded to open the port!") -else: - print("Failed to open the port!") - print("Press any key to terminate...") - getch() - quit() - - -# Set port1 baudrate -if dynamixel.setBaudRate(port_num1, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - -# Set port2 baudrate -if dynamixel.setBaudRate(port_num2, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#1 has been successfully connected") - -# Enable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#2 has been successfully connected") - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Write Dynamixel#1 goal position - dynamixel.write2ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - # Write Dynamixel#2 goal position - dynamixel.write2ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - while 1: - # Read present position - dxl1_present_position = dynamixel.read2ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - # Read present position - dxl2_present_position = dynamixel.read2ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - print("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position)) - - if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) or (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Disable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Close port -dynamixel.closePort(port_num1) - -dynamixel.closePort(port_num2) diff --git a/python/protocol1_0/read_write.py b/python/protocol1_0/read_write.py deleted file mode 100644 index 903cb84d..00000000 --- a/python/protocol1_0/read_write.py +++ /dev/null @@ -1,172 +0,0 @@ -#!/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) - -# -# ********* Read and Write Example ********* -# -# -# Available DXL model on this example : All models using Protocol 1.0 -# This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL. -# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel MX properties are already set as ## ID : 1 / Baudnum : 34 (Baudrate : 57600) -# - -import os - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_MX_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model -ADDR_MX_GOAL_POSITION = 30 -ADDR_MX_PRESENT_POSITION = 36 - -# Protocol version -PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel - -# Default setting -DXL_ID = 1 # Dynamixel ID: 1 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# 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 -DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 10 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl_present_position = 0 # Present position - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel has been successfully connected") - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Write goal position - dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - while 1: - # Read present position - dxl_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position)) - - if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol1_0/sync_write.py b/python/protocol1_0/sync_write.py deleted file mode 100644 index dc3b374d..00000000 --- a/python/protocol1_0/sync_write.py +++ /dev/null @@ -1,222 +0,0 @@ -#!/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) - -# -# ********* Sync Write Example ********* -# -# -# Available Dynamixel model on this example : All models using Protocol 1.0 -# This example is designed for using two Dynamixel MX-28, and an USB2DYNAMIXEL. -# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) -# - -import os, ctypes - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_MX_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model -ADDR_MX_GOAL_POSITION = 30 -ADDR_MX_PRESENT_POSITION = 36 - -# Data Byte Length -LEN_MX_GOAL_POSITION = 2 -LEN_MX_PRESENT_POSITION = 2 - -# Protocol version -PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel - -# Default setting -DXL1_ID = 1 # Dynamixel ID: 1 -DXL2_ID = 2 # Dynamixel ID: 2 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# 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 -DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 10 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -# Initialize Groupsyncwrite instance -group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION) - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl1_present_position = 0 # Present position -dxl2_present_position = 0 - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#1 has been successfully connected") - -# Enable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#2 has been successfully connected") - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Add Dynamixel#1 goal position value to the Syncwrite storage - dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL1_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value - print(dxl_addparam_result) - if dxl_addparam_result != 1: - print(dxl_addparam_result) - print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID)) - quit() - - # Add Dynamixel#2 goal position value to the Syncwrite parameter storage - dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value - if dxl_addparam_result != 1: - print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID)) - - # Syncwrite goal position - dynamixel.groupSyncWriteTxPacket(group_num) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - - # Clear syncwrite parameter storage - dynamixel.groupSyncWriteClearParam(group_num) - - while 1: - # Read Dynamixel#1 present position - dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - # Read Dynamixel#2 present position - dxl2_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - print("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position)) - - if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) or (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Disable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol2_0/broadcast_ping.py b/python/protocol2_0/broadcast_ping.py deleted file mode 100644 index 39a66a64..00000000 --- a/python/protocol2_0/broadcast_ping.py +++ /dev/null @@ -1,107 +0,0 @@ -#!/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) - -# -# ********* ping 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(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) -# - -import os, ctypes - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel - -# Default setting -DXL_ID = 1 # Dynamixel ID: 1 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" - -MAX_ID = 252 # Maximum ID 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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -dxl_comm_result = COMM_TX_FAIL # Communication result - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - -# Try to broadcast ping the Dynamixel -dynamixel.broadcastPing(port_num, PROTOCOL_VERSION) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - -print("Detected Dynamixel : ") -for id in range(0, MAX_ID): - if ctypes.c_ubyte(dynamixel.getBroadcastPingResult(port_num, PROTOCOL_VERSION, id)).value: - print("[ID:%03d]" % (id)) - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol2_0/bulk_read_write.py b/python/protocol2_0/bulk_read_write.py deleted file mode 100644 index a85d4c46..00000000 --- a/python/protocol2_0/bulk_read_write.py +++ /dev/null @@ -1,248 +0,0 @@ -#!/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) - -# -# ********* Bulk Read and Bulk Write Example ********* -# -# -# Available Dynamixel model on this example : All models using Protocol 2.0 -# This example is designed for using two Dynamixel PRO 54-200, and an USB2DYNAMIXEL. -# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel PRO properties are already set as %% ID : 1 and 2 / Baudnum : 1 (Baudrate : 57600) -# - -import os, ctypes - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_PRO_TORQUE_ENABLE = 562 # Control table address is different in Dynamixel model -ADDR_PRO_LED_RED = 563 -ADDR_PRO_GOAL_POSITION = 596 -ADDR_PRO_PRESENT_POSITION = 611 - -# Data Byte Length -LEN_PRO_LED_RED = 1 -LEN_PRO_GOAL_POSITION = 4 -LEN_PRO_PRESENT_POSITION = 4 - -# Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel - -# Default setting -DXL1_ID = 1 # Dynamixel ID: 1 -DXL2_ID = 2 # Dynamixel ID: 2 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# 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 -DXL_MINIMUM_POSITION_VALUE = -150000 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 150000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -# Initialize groupBulkWrite Struct -groupwrite_num = dynamixel.groupBulkWrite(port_num, PROTOCOL_VERSION) - -# Initialize Groupsyncwrite instance -groupread_num = dynamixel.groupBulkRead(port_num, PROTOCOL_VERSION) - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_addparam_result = 0 # AddParam result -dxl_getdata_result = 0 # GetParam result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl_led_value = [0, 255] # Dynamixel LED value for write -dxl1_present_position = 0 # Present position -dxl2_led_value_read = 0 # Dynamixel moving status - - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#1 has been successfully connected") - -# Enable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#2 has been successfully connected") - -# Add parameter storage for Dynamixel#1 present position value -dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkReadAddParam(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)).value -if dxl_addparam_result != 1: - print("[ID:%03d] groupBulkRead addparam failed" % (DXL1_ID)) - quit() - -# Add parameter storage for Dynamixel#2 present moving value -dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkReadAddParam(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)).value -if dxl_addparam_result != 1: - print("[ID:%03d] groupBulkRead addparam failed" % (DXL2_ID)) - quit() - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Add parameter storage for Dynamixel#1 goal position - dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkWriteAddParam(groupwrite_num, DXL1_ID, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION, dxl_goal_position[index], LEN_PRO_GOAL_POSITION)).value - if dxl_addparam_result != 1: - fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed", DXL1_ID) - quit() - - # Add parameter storage for Dynamixel#2 LED value - dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkWriteAddParam(groupwrite_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED, dxl_led_value[index], LEN_PRO_LED_RED)).value - if dxl_addparam_result != 1: - fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed", DXL2_ID) - quit() - - # Bulkwrite goal position and LED value - dynamixel.groupBulkWriteTxPacket(groupwrite_num) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - - # Clear bulkwrite parameter storage - dynamixel.groupBulkWriteClearParam(groupwrite_num) - - while 1: - # Bulkread present position and moving status - dynamixel.groupBulkReadTxRxPacket(groupread_num) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - - # Check if groupbulkread data of Dynamixel#1 is available - dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)).value - if dxl_getdata_result != 1: - print("[ID:%03d] groupBulkRead getdata failed" % (DXL1_ID)) - quit() - - # Check if groupbulkread data of Dynamixel#2 is available - dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)).value - if dxl_getdata_result != 1: - print("[ID:%03d] groupBulkRead getdata failed" % (DXL2_ID)) - quit() - - # Get Dynamixel#1 present position value - dxl1_present_position = dynamixel.groupBulkReadGetData(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) - - # Get Dynamixel#2 moving status value - dxl2_led_value_read = dynamixel.groupBulkReadGetData(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED) - - print("[ID:%03d] Present Position : %d \t [ID:%03d] LED Value: %d" % (DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_led_value_read)) - - if not (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Disable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol2_0/factory_reset.py b/python/protocol2_0/factory_reset.py deleted file mode 100644 index ac37fba4..00000000 --- a/python/protocol2_0/factory_reset.py +++ /dev/null @@ -1,186 +0,0 @@ -#!/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) - -# -# ********* Factory Reset 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(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) -# - -# Be aware that: -# This example resets all properties of Dynamixel to default values, such as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) -# - -import os - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -from time import sleep -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_PRO_BAUDRATE = 8 # Control table address is different in Dynamixel model - -# Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel - -# Default setting -DXL_ID = 1 # Dynamixel ID: 1 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode("utf-8")# Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" - -FACTORYRST_DEFAULTBAUDRATE = 57600 # Dynamixel baudrate set by factoryreset -NEW_BAUDNUM = 3 # New baudnum to recover Dynamixel baudrate as it was -OPERATION_MODE = 0x01 # 0xFF : reset all values - # 0x01 : reset all values except ID - # 0x02 : reset all values except ID and baudrate - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -dxl_comm_result = COMM_TX_FAIL # Communication result - -dxl_error = 0 # Dynamixel error -dxl_baudnum_read = 0 # Read baudnum - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Read present baudrate of the controller -print("Now the controller baudrate is : %d" % (dynamixel.getBaudRate(port_num))) - -# Try factoryreset -print("[ID:%03d] Try factoryreset : " % (DXL_ID)) -dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print("Aborted") - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - -# Wait for reset -print("Wait for reset...") -sleep(2) - -print("[ID:%03d] factoryReset Success!" % (DXL_ID)) - -# Set controller baudrate to dxl default baudrate -if dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE): - print("Succeed to change the controller baudrate to : %d" % (FACTORYRST_DEFAULTBAUDRATE)) -else: - print("Failed to change the controller baudrate") - getch() - quit() - -# Read Dynamixel baudnum -dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) - -# Write new baudnum -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE, NEW_BAUDNUM) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM)) - -# Set port baudrate to BAUDRATE -if dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeed to change the controller baudrate to : %d" % (BAUDRATE)) -else: - print("Failed to change the controller baudrate") - getch() - quit() - -sleep(0.2) - -# Read Dynamixel baudnum -dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) - - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol2_0/indirect_address.py b/python/protocol2_0/indirect_address.py deleted file mode 100644 index 0fb2eadd..00000000 --- a/python/protocol2_0/indirect_address.py +++ /dev/null @@ -1,321 +0,0 @@ -#!/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) - -# -# ********* Indirect Address 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(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) -# - -import os, ctypes - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address # Control table address is different in Dynamixel model -ADDR_PRO_INDIRECTADDRESS_FOR_WRITE = 49 # EEPROM region -ADDR_PRO_INDIRECTADDRESS_FOR_READ = 59 # EEPROM region -ADDR_PRO_TORQUE_ENABLE = 562 -ADDR_PRO_LED_RED = 563 -ADDR_PRO_GOAL_POSITION = 596 -ADDR_PRO_MOVING = 610 -ADDR_PRO_PRESENT_POSITION = 611 -ADDR_PRO_INDIRECTDATA_FOR_WRITE = 634 -ADDR_PRO_INDIRECTDATA_FOR_READ = 639 - -# Data Byte Length -LEN_PRO_LED_RED = 1 -LEN_PRO_GOAL_POSITION = 4 -LEN_PRO_MOVING = 1 -LEN_PRO_PRESENT_POSITION = 4 -LEN_PRO_INDIRECTDATA_FOR_WRITE = 5 -LEN_PRO_INDIRECTDATA_FOR_READ = 5 - -# Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel - -# Default setting -DXL_ID = 1 # Dynamixel ID: 1 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# 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 -DXL_MINIMUM_POSITION_VALUE = -150000 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 150000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -# Initialize Groupsyncwrite instance -groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE) - -# Initialize Groupsyncread Structs for Present Position -groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ) - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_addparam_result = 0 # AddParam result -dxl_getdata_result = 0 # GetParam result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl_moving = 0 # Dynamixel moving status -dxl_led_value = [0x00, 0xFF] # Dynamixel LED value -dxl_present_position = 0 # Present position - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Disable Dynamixel Torque : -# Indirect address would not accessible when the torque is already enabled -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel has been successfully connected") - -# INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Enable Dynamixel Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Add parameter storage for Dynamixel present position value -dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL_ID)).value -if dxl_addparam_result != 1: - print("[ID:%03d] groupSyncRead addparam failed" % (DXL_ID)) - quit() - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Add Dynamixel#1 goal position value to the Syncwrite storage - dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_goal_position[index], LEN_PRO_GOAL_POSITION)).value - print(dxl_addparam_result) - if dxl_addparam_result != 1: - print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID)) - quit() - - # Add Dynamixel#2 goal position value to the Syncwrite parameter storage - dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_led_value[index], LEN_PRO_LED_RED)).value - if dxl_addparam_result != 1: - print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID)) - quit() - - # Syncwrite goal position - dynamixel.groupSyncWriteTxPacket(groupwrite_num) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - - # Clear syncwrite parameter storage - dynamixel.groupSyncWriteClearParam(groupwrite_num) - - while 1: - # Syncread present position - dynamixel.groupSyncReadTxRxPacket(groupread_num) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - - # Check if groupsyncread data of Dynamixel#1 is available - dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION)).value - if dxl_getdata_result != 1: - print("[ID:%03d] groupSyncRead getdata failed" % (DXL_ID)) - quit() - - # Check if groupsyncread data of Dynamixel#2 is available - dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING)).value - if dxl_getdata_result != 1: - print("[ID:%03d] groupSyncRead getdata failed" % (DXL_ID)) - quit() - - # Get Dynamixel#1 present position value - dxl_present_position = dynamixel.groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION) - - # Get Dynamixel#2 present position value - dxl_moving = dynamixel.groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING) - - print("[ID:%03d] GoalPos:%d PresPos:%d IsMoving:%d" % (DXL_ID, dxl_goal_position[index], dxl_present_position, dxl_moving)) - - if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol2_0/multi_port.py b/python/protocol2_0/multi_port.py deleted file mode 100644 index 0cfc6369..00000000 --- a/python/protocol2_0/multi_port.py +++ /dev/null @@ -1,233 +0,0 @@ -#!/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) - -# -# ********* MultiPort Example ********* -# -# -# Available Dynamixel model on this example : All models using Protocol 2.0 -# This example is designed for using two Dynamixel PRO 54-200, and two USB2DYNAMIXELs. -# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) -# - -import os - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_PRO_TORQUE_ENABLE = 562 # Control table address is different in Dynamixel model -ADDR_PRO_GOAL_POSITION = 596 -ADDR_PRO_PRESENT_POSITION = 611 - -# Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel - -# Default setting -DXL1_ID = 1 # Dynamixel ID: 1 -DXL2_ID = 2 # Dynamixel ID: 2 -BAUDRATE = 57600 -DEVICENAME1 = "/dev/ttyUSB0".encode('utf-8')# Check which port is being used on your controller -DEVICENAME2 = "/dev/ttyUSB1".encode('utf-8')# 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 -DXL_MINIMUM_POSITION_VALUE = -150000 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 150000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 10 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -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_num1 = dynamixel.portHandler(DEVICENAME1) -port_num2 = dynamixel.portHandler(DEVICENAME2) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl1_present_position = 0 # Present position -dxl2_present_position = 0 - -# Open port1 -if dynamixel.openPort(port_num1): - print("Succeeded to open the port!") -else: - print("Failed to open the port!") - print("Press any key to terminate...") - getch() - quit() - -# Open port2 -if dynamixel.openPort(port_num2): - print("Succeeded to open the port!") -else: - print("Failed to open the port!") - print("Press any key to terminate...") - getch() - quit() - - -# Set port1 baudrate -if dynamixel.setBaudRate(port_num1, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - -# Set port2 baudrate -if dynamixel.setBaudRate(port_num2, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#1 has been successfully connected") - -# Enable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#2 has been successfully connected") - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Write Dynamixel#1 goal position - dynamixel.write4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - # Write Dynamixel#2 goal position - dynamixel.write4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - while 1: - # Read present position - dxl1_present_position = dynamixel.read4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - # Read present position - dxl2_present_position = dynamixel.read4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - print("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position)) - - if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) or (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Disable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Close port -dynamixel.closePort(port_num1) - -dynamixel.closePort(port_num2) diff --git a/python/protocol2_0/read_write.py b/python/protocol2_0/read_write.py deleted file mode 100644 index 4623c4ba..00000000 --- a/python/protocol2_0/read_write.py +++ /dev/null @@ -1,172 +0,0 @@ -#!/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) - -# -# ********* Read and Write 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(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) -# - -import os - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_PRO_TORQUE_ENABLE = 562 # Control table address is different in Dynamixel model -ADDR_PRO_GOAL_POSITION = 596 -ADDR_PRO_PRESENT_POSITION = 611 - -# Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel - -# Default setting -DXL_ID = 1 # Dynamixel ID: 1 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# 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 -DXL_MINIMUM_POSITION_VALUE = -150000 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 150000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl_present_position = 0 # Present position - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel has been successfully connected") - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Write goal position - dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - while 1: - # Read present position - dxl_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position)) - - if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol2_0/sync_read_write.py b/python/protocol2_0/sync_read_write.py deleted file mode 100644 index a9b2e7e6..00000000 --- a/python/protocol2_0/sync_read_write.py +++ /dev/null @@ -1,246 +0,0 @@ -#!/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) - -# -# ********* Sync Read and Sync Write Example ********* -# -# -# Available Dynamixel model on this example : All models using Protocol 2.0 -# This example is designed for using two Dynamixel PRO 54-200, and an USB2DYNAMIXEL. -# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. -# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) -# - -import os, ctypes - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address -ADDR_PRO_TORQUE_ENABLE = 562 # Control table address is different in Dynamixel model -ADDR_PRO_GOAL_POSITION = 596 -ADDR_PRO_PRESENT_POSITION = 611 - -# Data Byte Length -LEN_PRO_GOAL_POSITION = 4 -LEN_PRO_PRESENT_POSITION = 4 - -# Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel - -# Default setting -DXL1_ID = 1 # Dynamixel ID: 1 -DXL2_ID = 2 # Dynamixel ID: 2 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# 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 -DXL_MINIMUM_POSITION_VALUE = -150000 # Dynamixel will rotate between this value -DXL_MAXIMUM_POSITION_VALUE = 150000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold - -ESC_ASCII_VALUE = 0x1b - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -# Initialize Groupsyncwrite instance -groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION) - -# Initialize Groupsyncread Structs for Present Position -groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl_addparam_result = 0 # AddParam result -dxl_getdata_result = 0 # GetParam result -dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position - -dxl_error = 0 # Dynamixel error -dxl1_present_position = 0 # Present position -dxl2_present_position = 0 - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#1 has been successfully connected") - -# Enable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) -else: - print("Dynamixel#2 has been successfully connected") - -# Add parameter storage for Dynamixel#1 present position value -dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL1_ID)).value -if dxl_addparam_result != 1: - print("[ID:%03d] groupSyncRead addparam failed" % (DXL1_ID)) - quit() - -# Add parameter storage for Dynamixel#2 present position value -dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL2_ID)).value -if dxl_addparam_result != 1: - print("[ID:%03d] groupSyncRead addparam failed" % (DXL2_ID)) - quit() - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Add Dynamixel#1 goal position value to the Syncwrite storage - dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL1_ID, dxl_goal_position[index], LEN_PRO_GOAL_POSITION)).value - print(dxl_addparam_result) - if dxl_addparam_result != 1: - print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID)) - quit() - - # Add Dynamixel#2 goal position value to the Syncwrite parameter storage - dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL2_ID, dxl_goal_position[index], LEN_PRO_GOAL_POSITION)).value - if dxl_addparam_result != 1: - print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID)) - quit() - - # Syncwrite goal position - dynamixel.groupSyncWriteTxPacket(groupwrite_num) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - - # Clear syncwrite parameter storage - dynamixel.groupSyncWriteClearParam(groupwrite_num) - - while 1: - # Syncread present position - dynamixel.groupSyncReadTxRxPacket(groupread_num) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) - - # Check if groupsyncread data of Dynamixel#1 is available - dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupSyncReadIsAvailable(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)).value - if dxl_getdata_result != 1: - print("[ID:%03d] groupSyncRead getdata failed" % (DXL1_ID)) - quit() - - # Check if groupsyncread data of Dynamixel#2 is available - dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupSyncReadIsAvailable(groupread_num, DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)).value - if dxl_getdata_result != 1: - print("[ID:%03d] groupSyncRead getdata failed" % (DXL2_ID)) - quit() - - # Get Dynamixel#1 present position value - dxl1_present_position = dynamixel.groupSyncReadGetData(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) - - # Get Dynamixel#2 present position value - dxl2_present_position = dynamixel.groupSyncReadGetData(groupread_num, DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) - - print("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position)) - - if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) or (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Disable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) - -# Close port -dynamixel.closePort(port_num) diff --git a/python/protocol_combined/protocol_combined.py b/python/protocol_combined/protocol_combined.py deleted file mode 100644 index 965f0469..00000000 --- a/python/protocol_combined/protocol_combined.py +++ /dev/null @@ -1,226 +0,0 @@ -#!/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) - -# -# ********* Protocol Combined Example ********* -# -# -# Available Dynamixel model on this example : All models using Protocol 1.0 and 2.0 -# This example is tested with a Dynamixel MX-28, a Dynamixel PRO 54-200 and an USB2DYNAMIXEL -# Be sure that properties of Dynamixel MX and PRO are already set as %% MX - ID : 1 / Baudnum : 34 (Baudrate : 57600) , PRO - ID : 1 / Baudnum : 1 (Baudrate : 57600) -# - -# Be aware that: -# This example configures two different control tables (especially, if it uses Dynamixel and Dynamixel PRO). It may modify critical Dynamixel parameter on the control table, if Dynamixels have wrong ID. -# - -import os - -if os.name == 'nt': - import msvcrt - def getch(): - return msvcrt.getch().decode() -else: - import sys, tty, termios - fd = sys.stdin.fileno() - old_settings = termios.tcgetattr(fd) - def getch(): - try: - tty.setraw(sys.stdin.fileno()) - ch = sys.stdin.read(1) - finally: - termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) - return ch - -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library - -# Control table address for Dynamixel MX -ADDR_MX_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model -ADDR_MX_GOAL_POSITION = 30 -ADDR_MX_PRESENT_POSITION = 36 - -# Control table address for Dynamixel PRO -ADDR_PRO_TORQUE_ENABLE = 562 -ADDR_PRO_GOAL_POSITION = 596 -ADDR_PRO_PRESENT_POSITION = 611 - -# Protocol version -PROTOCOL_VERSION1 = 1 # See which protocol version is used in the Dynamixel -PROTOCOL_VERSION2 = 2 - -# Default setting -DXL1_ID = 1 # Dynamixel ID: 1 -DXL2_ID = 2 # Dynamixel ID: 2 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# 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 -DXL1_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value -DXL1_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) -DXL2_MINIMUM_POSITION_VALUE = -150000 -DXL2_MAXIMUM_POSITION_VALUE = 150000 -DXL1_MOVING_STATUS_THRESHOLD = 10 # Dynamixel moving status threshold -DXL2_MOVING_STATUS_THRESHOLD = 20 - -ESC_ASCII_VALUE = 0x1b - -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 = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() - -index = 0 -dxl_comm_result = COMM_TX_FAIL # Communication result -dxl1_goal_position = [DXL1_MINIMUM_POSITION_VALUE, DXL1_MAXIMUM_POSITION_VALUE] # Goal position of Dynamixel MX -dxl2_goal_position = [DXL2_MINIMUM_POSITION_VALUE, DXL2_MAXIMUM_POSITION_VALUE] # Goal position of Dynamixel PRO -dxl_error = 0 # Dynamixel error -dxl1_present_position = 0 # Present position of Dynamixel MX -dxl2_present_position = 0 # Present position of Dynamixel PRO - -# Open port -if dynamixel.openPort(port_num): - 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 dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") -else: - print("Failed to change the baudrate!") - print("Press any key to terminate...") - getch() - quit() - - -# Enable Dynamixel#1 torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION1, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION1, dxl_error)) -else: - print("Dynamixel#%d has been successfully connected" % (DXL1_ID)) - -# Enable Dynamixel#1 torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION2, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION2, dxl_error)) -else: - print("Dynamixel#%d has been successfully connected" % (DXL2_ID)) - - -while 1: - print("Press any key to continue! (or press ESC to quit!)") - if getch() == chr(ESC_ASCII_VALUE): - break - - # Write Dynamixel#1 goal position - dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl1_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION1, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION1, dxl_error)) - - # Write Dynamixel#1 goal position - dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_GOAL_POSITION, dxl2_goal_position[index]) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION2, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION2, dxl_error)) - - while 1: - # Read Dynamixel#1 present position - dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION1, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION1, dxl_error)) - - # Read Dynamixel#2 present position - dxl2_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_PRESENT_POSITION) - dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2) - dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2) - if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION2, dxl_comm_result)) - elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION2, dxl_error)) - - - print("[ID:%03d] GoalPos:%03d PresPos:%03d [ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl1_goal_position[index], dxl1_present_position, DXL2_ID, dxl2_goal_position[index], dxl2_present_position)) - - if not ((abs(dxl1_goal_position[index] - dxl1_present_position) > DXL1_MOVING_STATUS_THRESHOLD) or (abs(dxl2_goal_position[index] - dxl2_present_position) > DXL2_MOVING_STATUS_THRESHOLD)): - break - - # Change goal position - if index == 0: - index = 1 - else: - index = 0 - - -# Disable Dynamixel#1 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION1, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION1, dxl_error)) - -# Disable Dynamixel#2 Torque -dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2) -if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION2, dxl_comm_result)) -elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION2, dxl_error)) - - -# Close port -dynamixel.closePort(port_num) diff --git a/python/setup.py b/python/setup.py new file mode 100644 index 00000000..c10954e7 --- /dev/null +++ b/python/setup.py @@ -0,0 +1,17 @@ + +from setuptools import setup, find_packages +import platform + +setup( + name='dynamixel_sdk', + version='3.6.0', + packages=['dynamixel_sdk'], + package_dir={'': 'src'}, + license='Apache 2.0', + description='Dynamixel SDK 3. python package', + long_description=open('README.txt').read(), + url='https://github.com/ROBOTIS-GIT/DynamixelSDK', + author='Leon Jung', + author_email='rwjung@robotis.com', + install_requires=['pyserial'] +) diff --git a/python/src/dynamixel_sdk/__init__.py b/python/src/dynamixel_sdk/__init__.py new file mode 100644 index 00000000..bc227b66 --- /dev/null +++ b/python/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/python/src/dynamixel_sdk/group_bulk_read.py b/python/src/dynamixel_sdk/group_bulk_read.py new file mode 100644 index 00000000..2e17ffb3 --- /dev/null +++ b/python/src/dynamixel_sdk/group_bulk_read.py @@ -0,0 +1,147 @@ +#!/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 .robotis_def import * + +PARAM_NUM_DATA = 0 +PARAM_NUM_ADDRESS = 1 +PARAM_NUM_LENGTH = 2 + + +class GroupBulkRead: + def __init__(self, port, ph): + self.port = port + self.ph = ph + + self.last_result = False + self.is_param_changed = False + self.param = [] + self.data_dict = {} + + self.clearParam() + + def makeParam(self): + if not self.data_dict: + return + + self.param = [] + + for dxl_id in self.data_dict: + if self.ph.getProtocolVersion() == 1.0: + self.param.append(self.data_dict[dxl_id][2]) # LEN + self.param.append(dxl_id) # ID + self.param.append(self.data_dict[dxl_id][1]) # ADDR + else: + self.param.append(dxl_id) # ID + self.param.append(DXL_LOBYTE(self.data_dict[dxl_id][1])) # ADDR_L + self.param.append(DXL_HIBYTE(self.data_dict[dxl_id][1])) # ADDR_H + self.param.append(DXL_LOBYTE(self.data_dict[dxl_id][2])) # LEN_L + self.param.append(DXL_HIBYTE(self.data_dict[dxl_id][2])) # LEN_H + + def addParam(self, dxl_id, start_address, data_length): + if dxl_id in self.data_dict: # dxl_id already exist + return False + + data = [] # [0] * data_length + self.data_dict[dxl_id] = [data, start_address, data_length] + + self.is_param_changed = True + return True + + def removeParam(self, dxl_id): + if dxl_id not in self.data_dict: # NOT exist + return + + del self.data_dict[dxl_id] + + self.is_param_changed = True + + def clearParam(self): + self.data_dict.clear() + return + + def txPacket(self): + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or not self.param: + self.makeParam() + + if self.ph.getProtocolVersion() == 1.0: + return self.ph.bulkReadTx(self.port, self.param, len(self.data_dict.keys()) * 3) + else: + return self.ph.bulkReadTx(self.port, self.param, len(self.data_dict.keys()) * 5) + + def rxPacket(self): + self.last_result = False + + result = COMM_RX_FAIL + + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + for dxl_id in self.data_dict: + self.data_dict[dxl_id][PARAM_NUM_DATA], result, _ = self.ph.readRx(self.port, dxl_id, + self.data_dict[dxl_id][PARAM_NUM_LENGTH]) + if result != COMM_SUCCESS: + return result + + if result == COMM_SUCCESS: + self.last_result = True + + return result + + def txRxPacket(self): + result = self.txPacket() + if result != COMM_SUCCESS: + return result + + return self.rxPacket() + + def isAvailable(self, dxl_id, address, data_length): + if self.last_result is False or dxl_id not in self.data_dict: + return False + + start_addr = self.data_dict[dxl_id][PARAM_NUM_ADDRESS] + + if (address < start_addr) or (start_addr + self.data_dict[dxl_id][PARAM_NUM_LENGTH] - data_length < address): + return False + + return True + + def getData(self, dxl_id, address, data_length): + if not self.isAvailable(dxl_id, address, data_length): + return 0 + + start_addr = self.data_dict[dxl_id][PARAM_NUM_ADDRESS] + + if data_length == 1: + return self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr] + elif data_length == 2: + return DXL_MAKEWORD(self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr], + self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 1]) + elif data_length == 4: + return DXL_MAKEDWORD(DXL_MAKEWORD(self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 0], + self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 1]), + DXL_MAKEWORD(self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 2], + self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr + 3])) + else: + return 0 diff --git a/python/src/dynamixel_sdk/group_bulk_write.py b/python/src/dynamixel_sdk/group_bulk_write.py new file mode 100644 index 00000000..fa7bbb79 --- /dev/null +++ b/python/src/dynamixel_sdk/group_bulk_write.py @@ -0,0 +1,109 @@ +#!/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 .robotis_def import * + + +class GroupBulkWrite: + def __init__(self, port, ph): + self.port = port + self.ph = ph + + self.is_param_changed = False + self.param = [] + self.data_list = {} + + self.clearParam() + + def makeParam(self): + if self.ph.getProtocolVersion() == 1.0 or not self.data_list: + return + + self.param = [] + + for dxl_id in self.data_list: + if not self.data_list[dxl_id]: + return + + self.param.append(dxl_id) + self.param.append(DXL_LOBYTE(self.data_list[dxl_id][1])) + self.param.append(DXL_HIBYTE(self.data_list[dxl_id][1])) + self.param.append(DXL_LOBYTE(self.data_list[dxl_id][2])) + self.param.append(DXL_HIBYTE(self.data_list[dxl_id][2])) + + self.param.extend(self.data_list[dxl_id][0]) + + def addParam(self, dxl_id, start_address, data_length, data): + if self.ph.getProtocolVersion() == 1.0: + return False + + if dxl_id in self.data_list: # dxl_id already exist + return False + + if len(data) > data_length: # input data is longer than set + return False + + self.data_list[dxl_id] = [data, start_address, data_length] + + self.is_param_changed = True + return True + + def removeParam(self, dxl_id): + if self.ph.getProtocolVersion() == 1.0: + return + + if dxl_id not in self.data_list: # NOT exist + return + + del self.data_list[dxl_id] + + self.is_param_changed = True + + def changeParam(self, dxl_id, start_address, data_length, data): + if self.ph.getProtocolVersion() == 1.0: + return False + + if dxl_id not in self.data_list: # NOT exist + return False + + if len(data) > data_length: # input data is longer than set + return False + + self.data_list[dxl_id] = [data, start_address, data_length] + + self.is_param_changed = True + return True + + def clearParam(self): + if self.ph.getProtocolVersion() == 1.0: + return + + self.data_list.clear() + return + + def txPacket(self): + if self.ph.getProtocolVersion() == 1.0 or len(self.data_list.keys()) == 0: + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or len(self.param) == 0: + self.makeParam() + + return self.ph.bulkWriteTxOnly(self.port, self.param, len(self.param)) diff --git a/python/src/dynamixel_sdk/group_sync_read.py b/python/src/dynamixel_sdk/group_sync_read.py new file mode 100644 index 00000000..8a3441a5 --- /dev/null +++ b/python/src/dynamixel_sdk/group_sync_read.py @@ -0,0 +1,145 @@ +#!/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 .robotis_def import * + + +class GroupSyncRead: + def __init__(self, port, ph, start_address, data_length): + self.port = port + self.ph = ph + self.start_address = start_address + self.data_length = data_length + + self.last_result = False + self.is_param_changed = False + self.param = [] + self.data_dict = {} + + self.clearParam() + + def makeParam(self): + if self.ph.getProtocolVersion() == 1.0: + return + + if not self.data_dict: # len(self.data_dict.keys()) == 0: + return + + self.param = [] + + for dxl_id in self.data_dict: + self.param.append(dxl_id) + + def addParam(self, dxl_id): + if self.ph.getProtocolVersion() == 1.0: + return False + + if dxl_id in self.data_dict: # dxl_id already exist + return False + + self.data_dict[dxl_id] = [] # [0] * self.data_length + + self.is_param_changed = True + return True + + def removeParam(self, dxl_id): + if self.ph.getProtocolVersion() == 1.0: + return + + if dxl_id not in self.data_dict: # NOT exist + return + + del self.data_dict[dxl_id] + + self.is_param_changed = True + + def clearParam(self): + if self.ph.getProtocolVersion() == 1.0: + return + + self.data_dict.clear() + + def txPacket(self): + if self.ph.getProtocolVersion() == 1.0 or len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or not self.param: + self.makeParam() + + return self.ph.syncReadTx(self.port, self.start_address, self.data_length, self.param, + len(self.data_dict.keys()) * 1) + + def rxPacket(self): + self.last_result = False + + if self.ph.getProtocolVersion() == 1.0: + return COMM_NOT_AVAILABLE + + result = COMM_RX_FAIL + + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + for dxl_id in self.data_dict: + self.data_dict[dxl_id], result, _ = self.ph.readRx(self.port, dxl_id, self.data_length) + if result != COMM_SUCCESS: + return result + + if result == COMM_SUCCESS: + self.last_result = True + + return result + + def txRxPacket(self): + if self.ph.getProtocolVersion() == 1.0: + return COMM_NOT_AVAILABLE + + result = self.txPacket() + if result != COMM_SUCCESS: + return result + + return self.rxPacket() + + def isAvailable(self, dxl_id, address, data_length): + if self.ph.getProtocolVersion() == 1.0 or self.last_result is False or dxl_id not in self.data_dict: + return False + + if (address < self.start_address) or (self.start_address + self.data_length - data_length < address): + return False + + return True + + def getData(self, dxl_id, address, data_length): + if not self.isAvailable(dxl_id, address, data_length): + return 0 + + if data_length == 1: + return self.data_dict[dxl_id][address - self.start_address] + elif data_length == 2: + return DXL_MAKEWORD(self.data_dict[dxl_id][address - self.start_address], + self.data_dict[dxl_id][address - self.start_address + 1]) + elif data_length == 4: + return DXL_MAKEDWORD(DXL_MAKEWORD(self.data_dict[dxl_id][address - self.start_address + 0], + self.data_dict[dxl_id][address - self.start_address + 1]), + DXL_MAKEWORD(self.data_dict[dxl_id][address - self.start_address + 2], + self.data_dict[dxl_id][address - self.start_address + 3])) + else: + return 0 diff --git a/python/src/dynamixel_sdk/group_sync_write.py b/python/src/dynamixel_sdk/group_sync_write.py new file mode 100644 index 00000000..c9314e8f --- /dev/null +++ b/python/src/dynamixel_sdk/group_sync_write.py @@ -0,0 +1,94 @@ +#!/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 .robotis_def import * + + +class GroupSyncWrite: + def __init__(self, port, ph, start_address, data_length): + self.port = port + self.ph = ph + self.start_address = start_address + self.data_length = data_length + + self.is_param_changed = False + self.param = [] + self.data_dict = {} + + self.clearParam() + + def makeParam(self): + if not self.data_dict: + return + + self.param = [] + + for dxl_id in self.data_dict: + if not self.data_dict[dxl_id]: + return + + self.param.append(dxl_id) + self.param.extend(self.data_dict[dxl_id]) + + def addParam(self, dxl_id, data): + if dxl_id in self.data_dict: # dxl_id already exist + return False + + if len(data) > self.data_length: # input data is longer than set + return False + + self.data_dict[dxl_id] = data + + self.is_param_changed = True + return True + + def removeParam(self, dxl_id): + if dxl_id not in self.data_dict: # NOT exist + return + + del self.data_dict[dxl_id] + + self.is_param_changed = True + + def changeParam(self, dxl_id, data): + if dxl_id not in self.data_dict: # NOT exist + return False + + if len(data) > self.data_length: # input data is longer than set + return False + + self.data_dict[dxl_id] = data + + self.is_param_changed = True + return True + + def clearParam(self): + self.data_dict.clear() + + def txPacket(self): + if len(self.data_dict.keys()) == 0: + return COMM_NOT_AVAILABLE + + if self.is_param_changed is True or not self.param: + self.makeParam() + + return self.ph.syncWriteTxOnly(self.port, self.start_address, self.data_length, self.param, + len(self.data_dict.keys()) * (1 + self.data_length)) diff --git a/python/src/dynamixel_sdk/packet_handler.py b/python/src/dynamixel_sdk/packet_handler.py new file mode 100644 index 00000000..e6620c80 --- /dev/null +++ b/python/src/dynamixel_sdk/packet_handler.py @@ -0,0 +1,33 @@ +#!/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 .protocol1_packet_handler import * +from .protocol2_packet_handler import * + + +def PacketHandler(protocol_version): + # FIXME: float or int-to-float comparison can generate weird behaviour + if protocol_version == 1.0: + return Protocol1PacketHandler() + elif protocol_version == 2.0: + return Protocol2PacketHandler() + else: + return Protocol2PacketHandler() diff --git a/python/src/dynamixel_sdk/port_handler.py b/python/src/dynamixel_sdk/port_handler.py new file mode 100644 index 00000000..de555d45 --- /dev/null +++ b/python/src/dynamixel_sdk/port_handler.py @@ -0,0 +1,142 @@ +#!/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) + +import time +import serial +import sys +import platform + +LATENCY_TIMER = 16 +DEFAULT_BAUDRATE = 1000000 + + +class PortHandler(object): + def __init__(self, port_name): + self.is_open = False + self.baudrate = DEFAULT_BAUDRATE + self.packet_start_time = 0.0 + self.packet_timeout = 0.0 + self.tx_time_per_byte = 0.0 + + self.is_using = False + self.port_name = port_name + self.ser = None + + def openPort(self): + return self.setBaudRate(self.baudrate) + + def closePort(self): + self.ser.close() + self.is_open = False + + def clearPort(self): + self.ser.flush() + + def setPortName(self, port_name): + self.port_name = port_name + + def getPortName(self): + return self.port_name + + def setBaudRate(self, baudrate): + baud = self.getCFlagBaud(baudrate) + + if baud <= 0: + # self.setupPort(38400) + # self.baudrate = baudrate + return False # TODO: setCustomBaudrate(baudrate) + else: + self.baudrate = baudrate + return self.setupPort(baud) + + def getBaudRate(self): + return self.baudrate + + def getBytesAvailable(self): + return self.ser.in_waiting + + def readPort(self, length): + if (sys.version_info > (3, 0)): + return self.ser.read(length) + else: + return [ord(ch) for ch in self.ser.read(length)] + + def writePort(self, packet): + return self.ser.write(packet) + + def setPacketTimeout(self, packet_length): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = (self.tx_time_per_byte * packet_length) + (LATENCY_TIMER * 2.0) + 2.0 + + def setPacketTimeoutMillis(self, msec): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = msec + + def isPacketTimeout(self): + if self.getTimeSinceStart() > self.packet_timeout: + self.packet_timeout = 0 + return True + + return False + + def getCurrentTime(self): + return round(time.time() * 1000000000) / 1000000.0 + + def getTimeSinceStart(self): + time_since = self.getCurrentTime() - self.packet_start_time + if time_since < 0.0: + self.packet_start_time = self.getCurrentTime() + + return time_since + + def setupPort(self, cflag_baud): + if self.is_open: + self.closePort() + + self.ser = serial.Serial( + port=self.port_name, + baudrate=self.baudrate, + # parity = serial.PARITY_ODD, + # stopbits = serial.STOPBITS_TWO, + bytesize=serial.EIGHTBITS, + timeout=0 + ) + + self.is_open = True + + self.ser.reset_input_buffer() + + self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 + + return True + + def getCFlagBaud(self, baudrate): + if platform.system() == 'Darwin': + if baudrate in [9600, 19200, 38400, 57600, 115200]: + return baudrate + else: + return -1 + 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 diff --git a/python/src/dynamixel_sdk/protocol1_packet_handler.py b/python/src/dynamixel_sdk/protocol1_packet_handler.py new file mode 100644 index 00000000..768c6f00 --- /dev/null +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -0,0 +1,523 @@ +#!/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 .robotis_def import * + +TXPACKET_MAX_LEN = 250 +RXPACKET_MAX_LEN = 250 + +# for Protocol 1.0 Packet +PKT_HEADER0 = 0 +PKT_HEADER1 = 1 +PKT_ID = 2 +PKT_LENGTH = 3 +PKT_INSTRUCTION = 4 +PKT_ERROR = 4 +PKT_PARAMETER0 = 5 + +# Protocol 1.0 Error bit +ERRBIT_VOLTAGE = 1 # Supplied voltage is out of the range (operating volatage set in the control table) +ERRBIT_ANGLE = 2 # Goal position is written out of the range (from CW angle limit to CCW angle limit) +ERRBIT_OVERHEAT = 4 # Temperature is out of the range (operating temperature set in the control table) +ERRBIT_RANGE = 8 # Command(setting value) is out of the range for use. +ERRBIT_CHECKSUM = 16 # Instruction packet checksum is incorrect. +ERRBIT_OVERLOAD = 32 # The current load cannot be controlled by the set torque. +ERRBIT_INSTRUCTION = 64 # Undefined instruction or delivering the action command without the reg_write command. + + +class Protocol1PacketHandler(object): + def getProtocolVersion(self): + return 1.0 + + def getTxRxResult(self, result): + if result == COMM_SUCCESS: + return "[TxRxResult] Communication success!" + elif result == COMM_PORT_BUSY: + return "[TxRxResult] Port is in use!" + elif result == COMM_TX_FAIL: + return "[TxRxResult] Failed transmit instruction packet!" + elif result == COMM_RX_FAIL: + return "[TxRxResult] Failed get status packet from device!" + elif result == COMM_TX_ERROR: + return "[TxRxResult] Incorrect instruction packet!" + elif result == COMM_RX_WAITING: + return "[TxRxResult] Now receiving status packet!" + elif result == COMM_RX_TIMEOUT: + return "[TxRxResult] There is no status packet!" + elif result == COMM_RX_CORRUPT: + return "[TxRxResult] Incorrect status packet!" + elif result == COMM_NOT_AVAILABLE: + return "[TxRxResult] Protocol does not support this function!" + else: + return "" + + def getRxPacketError(self, error): + if error & ERRBIT_VOLTAGE: + return "[RxPacketError] Input voltage error!" + + if error & ERRBIT_ANGLE: + return "[RxPacketError] Angle limit error!" + + if error & ERRBIT_OVERHEAT: + return "[RxPacketError] Overheat error!" + + if error & ERRBIT_RANGE: + return "[RxPacketError] Out of range error!" + + if error & ERRBIT_CHECKSUM: + return "[RxPacketError] Checksum error!" + + if error & ERRBIT_OVERLOAD: + return "[RxPacketError] Overload error!" + + if error & ERRBIT_INSTRUCTION: + return "[RxPacketError] Instruction code error!" + + return "" + + def txPacket(self, port, txpacket): + checksum = 0 + total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH + + if port.is_using: + return COMM_PORT_BUSY + port.is_using = True + + # check max packet length + if total_packet_length > TXPACKET_MAX_LEN: + port.is_using = False + return COMM_TX_ERROR + + # make packet header + txpacket[PKT_HEADER0] = 0xFF + txpacket[PKT_HEADER1] = 0xFF + + # add a checksum to the packet + for idx in range(2, total_packet_length - 1): # except header, checksum + checksum += txpacket[idx] + + txpacket[total_packet_length - 1] = ~checksum & 0xFF + + #print "[TxPacket] %r" % txpacket + + # tx packet + port.clearPort() + written_packet_length = port.writePort(txpacket) + if total_packet_length != written_packet_length: + port.is_using = False + return COMM_TX_FAIL + + return COMM_SUCCESS + + def rxPacket(self, port): + rxpacket = [] + + result = COMM_TX_FAIL + checksum = 0 + rx_length = 0 + wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) + + while True: + rxpacket.extend(port.readPort(wait_length - rx_length)) + rx_length = len(rxpacket) + if rx_length >= wait_length: + # find packet header + for idx in range(0, (rx_length - 1)): + if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF): + break + + if idx == 0: # found at the beginning of the packet + if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or ( + rxpacket[PKT_ERROR] > 0x7F): + # unavailable ID or unavailable Length or unavailable Error + # remove the first byte in the packet + del rxpacket[0] + rx_length -= 1 + continue + + # re-calculate the exact length of the rx packet + if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1): + wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1 + continue + + if rx_length < wait_length: + # check timeout + if port.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + else: + continue + + # calculate checksum + for i in range(2, wait_length - 1): # except header, checksum + checksum += rxpacket[i] + checksum = ~checksum & 0xFF + + # verify checksum + if rxpacket[wait_length - 1] == checksum: + result = COMM_SUCCESS + else: + result = COMM_RX_CORRUPT + break + + else: + # remove unnecessary packets + del rxpacket[0: idx] + rx_length -= idx + + else: + # check timeout + if port.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + + port.is_using = False + + #print "[RxPacket] %r" % rxpacket + + return rxpacket, result + + # NOT for BulkRead + def txRxPacket(self, port, txpacket): + rxpacket = None + error = 0 + + # tx packet + result = self.txPacket(port, txpacket) + if result != COMM_SUCCESS: + return rxpacket, result, error + + # (Instruction == BulkRead) == this function is not available. + if txpacket[PKT_INSTRUCTION] == INST_BULK_READ: + result = COMM_NOT_AVAILABLE + + # (ID == Broadcast ID) == no need to wait for status packet or not available + if (txpacket[PKT_ID] == BROADCAST_ID): + port.is_using = False + return rxpacket, result, error + + # set packet timeout + if txpacket[PKT_INSTRUCTION] == INST_READ: + port.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6) + else: + port.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + + # rx packet + while True: + rxpacket, result = self.rxPacket(port) + if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: + break + + if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: + error = rxpacket[PKT_ERROR] + + return rxpacket, result, error + + def ping(self, port, dxl_id): + model_number = 0 + error = 0 + + txpacket = [0] * 6 + + if dxl_id >= BROADCAST_ID: + return model_number, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_PING + + rxpacket, result, error = self.txRxPacket(port, txpacket) + + if result == COMM_SUCCESS: + data_read, result, error = self.readTxRx(port, dxl_id, 0, 2) # Address 0 : Model Number + if result == COMM_SUCCESS: + model_number = DXL_MAKEWORD(data_read[0], data_read[1]) + + return model_number, result, error + + def broadcastPing(self, port): + data_list = None + return data_list, COMM_NOT_AVAILABLE + + def action(self, port, dxl_id): + txpacket = [0] * 6 + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_ACTION + + _, result, _ = self.txRxPacket(port, txpacket) + + return result + + def reboot(self, port, dxl_id): + return COMM_NOT_AVAILABLE, 0 + + def factoryReset(self, port, dxl_id): + txpacket = [0] * 6 + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET + + _, result, error = self.txRxPacket(port, txpacket) + + return result, error + + def readTx(self, port, dxl_id, address, length): + + txpacket = [0] * 8 + + if dxl_id >= BROADCAST_ID: + return COMM_NOT_AVAILABLE + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = 4 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length + + result = self.txPacket(port, txpacket) + + # set packet timeout + if result == COMM_SUCCESS: + port.setPacketTimeout(length + 6) + + return result + + def readRx(self, port, dxl_id, length): + result = COMM_TX_FAIL + error = 0 + + rxpacket = None + data = [] + + while True: + rxpacket, result = self.rxPacket(port) + + if result != COMM_SUCCESS or rxpacket[PKT_ID] == dxl_id: + break + + if result == COMM_SUCCESS and rxpacket[PKT_ID] == dxl_id: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) + + return data, result, error + + def readTxRx(self, port, dxl_id, address, length): + txpacket = [0] * 8 + data = [] + + if dxl_id >= BROADCAST_ID: + return data, COMM_NOT_AVAILABLE, 0 + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = 4 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length + + rxpacket, result, error = self.txRxPacket(port, txpacket) + if result == COMM_SUCCESS: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) + + return data, result, error + + def read1ByteTx(self, port, dxl_id, address): + return self.readTx(port, dxl_id, address, 1) + + def read1ByteRx(self, port, dxl_id): + data, result, error = self.readRx(port, dxl_id, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read1ByteTxRx(self, port, dxl_id, address): + data, result, error = self.readTxRx(port, dxl_id, address, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTx(self, port, dxl_id, address): + return self.readTx(port, dxl_id, address, 2) + + def read2ByteRx(self, port, dxl_id): + data, result, error = self.readRx(port, dxl_id, 2) + data_read = DXL_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTxRx(self, port, dxl_id, address): + data, result, error = self.readTxRx(port, dxl_id, address, 2) + data_read = DXL_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTx(self, port, dxl_id, address): + return self.readTx(port, dxl_id, address, 4) + + def read4ByteRx(self, port, dxl_id): + data, result, error = self.readRx(port, dxl_id, 4) + data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), + DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTxRx(self, port, dxl_id, address): + data, result, error = self.readTxRx(port, dxl_id, address, 4) + data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), + DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def writeTxOnly(self, port, dxl_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + result = self.txPacket(port, txpacket) + port.is_using = False + + return result + + def writeTxRx(self, port, dxl_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + rxpacket, result, error = self.txRxPacket(port, txpacket) + + return result, error + + def write1ByteTxOnly(self, port, dxl_id, address, data): + data_write = [data] + return self.writeTxOnly(port, dxl_id, address, 1, data_write) + + def write1ByteTxRx(self, port, dxl_id, address, data): + data_write = [data] + return self.writeTxRx(port, dxl_id, address, 1, data_write) + + def write2ByteTxOnly(self, port, dxl_id, address, data): + data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] + return self.writeTxOnly(port, dxl_id, address, 2, data_write) + + def write2ByteTxRx(self, port, dxl_id, address, data): + data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] + return self.writeTxRx(port, dxl_id, address, 2, data_write) + + def write4ByteTxOnly(self, port, dxl_id, address, data): + data_write = [DXL_LOBYTE(DXL_LOWORD(data)), + DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), + DXL_HIBYTE(DXL_HIWORD(data))] + return self.writeTxOnly(port, dxl_id, address, 4, data_write) + + def write4ByteTxRx(self, port, dxl_id, address, data): + data_write = [DXL_LOBYTE(DXL_LOWORD(data)), + DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), + DXL_HIBYTE(DXL_HIWORD(data))] + return self.writeTxRx(port, dxl_id, address, 4, data_write) + + def regWriteTxOnly(self, port, dxl_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + result = self.txPacket(port, txpacket) + port.is_using = False + + return result + + def regWriteTxRx(self, port, dxl_id, address, length, data): + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = length + 3 + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0] = address + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] + + _, result, error = self.txRxPacket(port, txpacket) + + return result, error + + def syncReadTx(self, port, start_address, data_length, param, param_length): + return COMM_NOT_AVAILABLE + + def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): + txpacket = [0] * (param_length + 8) + # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID + txpacket[PKT_LENGTH] = param_length + 4 # 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE + txpacket[PKT_PARAMETER0 + 0] = start_address + txpacket[PKT_PARAMETER0 + 1] = data_length + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] + + _, result, _ = self.txRxPacket(port, txpacket) + + return result + + def bulkReadTx(self, port, param, param_length): + txpacket = [0] * (param_length + 7) + # 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID + txpacket[PKT_LENGTH] = param_length + 3 # 3: INST 0x00 ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_BULK_READ + txpacket[PKT_PARAMETER0 + 0] = 0x00 + + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + param_length] = param[0: param_length] + + result = self.txPacket(port, txpacket) + if result == COMM_SUCCESS: + wait_length = 0 + i = 0 + while i < param_length: + wait_length += param[i] + 7 + i += 3 + port.setPacketTimeout(wait_length) + + return result + + def bulkWriteTxOnly(self, port, param, param_length): + return COMM_NOT_AVAILABLE diff --git a/python/src/dynamixel_sdk/protocol2_packet_handler.py b/python/src/dynamixel_sdk/protocol2_packet_handler.py new file mode 100644 index 00000000..a3df5ea8 --- /dev/null +++ b/python/src/dynamixel_sdk/protocol2_packet_handler.py @@ -0,0 +1,771 @@ +#!/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 .robotis_def import * + +TXPACKET_MAX_LEN = 4 * 1024 +RXPACKET_MAX_LEN = 4 * 1024 + +# for Protocol 2.0 Packet +PKT_HEADER0 = 0 +PKT_HEADER1 = 1 +PKT_HEADER2 = 2 +PKT_RESERVED = 3 +PKT_ID = 4 +PKT_LENGTH_L = 5 +PKT_LENGTH_H = 6 +PKT_INSTRUCTION = 7 +PKT_ERROR = 8 +PKT_PARAMETER0 = 8 + +# Protocol 2.0 Error bit +ERRNUM_RESULT_FAIL = 1 # Failed to process the instruction packet. +ERRNUM_INSTRUCTION = 2 # Instruction error +ERRNUM_CRC = 3 # CRC check error +ERRNUM_DATA_RANGE = 4 # Data range error +ERRNUM_DATA_LENGTH = 5 # Data length error +ERRNUM_DATA_LIMIT = 6 # Data limit error +ERRNUM_ACCESS = 7 # Access error + +ERRBIT_ALERT = 128 # When the device has a problem, this bit is set to 1. Check "Device Status Check" value. + + +class Protocol2PacketHandler(object): + def getProtocolVersion(self): + return 2.0 + + def getTxRxResult(self, result): + if result == COMM_SUCCESS: + return "[TxRxResult] Communication success!" + elif result == COMM_PORT_BUSY: + return "[TxRxResult] Port is in use!" + elif result == COMM_TX_FAIL: + return "[TxRxResult] Failed transmit instruction packet!" + elif result == COMM_RX_FAIL: + return "[TxRxResult] Failed get status packet from device!" + elif result == COMM_TX_ERROR: + return "[TxRxResult] Incorrect instruction packet!" + elif result == COMM_RX_WAITING: + return "[TxRxResult] Now receiving status packet!" + elif result == COMM_RX_TIMEOUT: + return "[TxRxResult] There is no status packet!" + elif result == COMM_RX_CORRUPT: + return "[TxRxResult] Incorrect status packet!" + elif result == COMM_NOT_AVAILABLE: + return "[TxRxResult] Protocol does not support this function!" + else: + return "" + + def getRxPacketError(self, error): + if error & ERRBIT_ALERT: + return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!" + + not_alert_error = error & ~ERRBIT_ALERT + if not_alert_error == 0: + return "" + elif not_alert_error == ERRNUM_RESULT_FAIL: + return "[RxPacketError] Failed to process the instruction packet!" + + elif not_alert_error == ERRNUM_INSTRUCTION: + return "[RxPacketError] Undefined instruction or incorrect instruction!" + + elif not_alert_error == ERRNUM_CRC: + return "[RxPacketError] CRC doesn't match!" + + elif not_alert_error == ERRNUM_DATA_RANGE: + return "[RxPacketError] The data value is out of range!" + + elif not_alert_error == ERRNUM_DATA_LENGTH: + return "[RxPacketError] The data length does not match as expected!" + + elif not_alert_error == ERRNUM_DATA_LIMIT: + return "[RxPacketError] The data value exceeds the limit value!" + + elif not_alert_error == ERRNUM_ACCESS: + return "[RxPacketError] Writing or Reading is not available to target address!" + + else: + return "[RxPacketError] Unknown error code!" + + def updateCRC(self, crc_accum, data_blk_ptr, data_blk_size): + crc_table = [0x0000, + 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, + 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, + 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, + 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, + 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, + 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, + 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, + 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, + 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, + 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, + 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, + 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, + 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, + 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, + 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, + 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, + 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, + 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, + 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, + 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, + 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, + 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, + 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, + 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, + 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, + 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, + 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, + 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, + 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, + 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, + 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, + 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, + 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, + 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, + 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, + 0x820D, 0x8207, 0x0202] + + for j in range(0, data_blk_size): + i = ((crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF + crc_accum = ((crc_accum << 8) ^ crc_table[i]) & 0xFFFF + + return crc_accum + + def addStuffing(self, packet): + packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]) + packet_length_out = packet_length_in + + temp = [0] * TXPACKET_MAX_LEN + + # FF FF FD XX ID LEN_L LEN_H + temp[PKT_HEADER0: PKT_HEADER0 + PKT_LENGTH_H + 1] = packet[PKT_HEADER0: PKT_HEADER0 + PKT_LENGTH_H + 1] + + index = PKT_INSTRUCTION + + for i in range(0, packet_length_in - 2): # except CRC + temp[index] = packet[i + PKT_INSTRUCTION] + index = index + 1 + if packet[i + PKT_INSTRUCTION] == 0xFD \ + and packet[i + PKT_INSTRUCTION - 1] == 0xFF \ + and packet[i + PKT_INSTRUCTION - 2] == 0xFF: + # FF FF FD + temp[index] = 0xFD + index = index + 1 + packet_length_out = packet_length_out + 1 + + temp[index] = packet[PKT_INSTRUCTION + packet_length_in - 2] + temp[index + 1] = packet[PKT_INSTRUCTION + packet_length_in - 1] + index = index + 2 + + if packet_length_in != packet_length_out: + packet = [0] * index + + packet[0: index] = temp[0: index] + + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out) + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out) + + return packet + + def removeStuffing(self, packet): + packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]) + packet_length_out = packet_length_in + + index = PKT_INSTRUCTION + for i in range(0, (packet_length_in - 2)): # except CRC + if (packet[i + PKT_INSTRUCTION] == 0xFD) and (packet[i + PKT_INSTRUCTION + 1] == 0xFD) and ( + 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 + + packet[index] = packet[PKT_INSTRUCTION + packet_length_in - 2] + packet[index + 1] = packet[PKT_INSTRUCTION + packet_length_in - 1] + + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out) + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out) + + return packet + + def txPacket(self, port, txpacket): + if port.is_using: + return COMM_PORT_BUSY + port.is_using = True + + # byte stuffing for header + self.addStuffing(txpacket) + + # check max packet length + total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7 + # 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H + + if total_packet_length > TXPACKET_MAX_LEN: + port.is_using = False + return COMM_TX_ERROR + + # make packet header + txpacket[PKT_HEADER0] = 0xFF + txpacket[PKT_HEADER1] = 0xFF + txpacket[PKT_HEADER2] = 0xFD + txpacket[PKT_RESERVED] = 0x00 + + # add CRC16 + crc = self.updateCRC(0, txpacket, total_packet_length - 2) # 2: CRC16 + + txpacket[total_packet_length - 2] = DXL_LOBYTE(crc) + txpacket[total_packet_length - 1] = DXL_HIBYTE(crc) + + # tx packet + port.clearPort() + written_packet_length = port.writePort(txpacket) + if total_packet_length != written_packet_length: + port.is_using = False + return COMM_TX_FAIL + + return COMM_SUCCESS + + def rxPacket(self, port): + rxpacket = [] + + result = COMM_TX_FAIL + rx_length = 0 + wait_length = 11 # minimum length (HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H) + + while True: + rxpacket.extend(port.readPort(wait_length - rx_length)) + rx_length = len(rxpacket) + if rx_length >= wait_length: + # find packet header + for idx in range(0, (rx_length - 3)): + if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF) and (rxpacket[idx + 2] == 0xFD) and ( + rxpacket[idx + 3] != 0xFD): + break + + if idx == 0: + if (rxpacket[PKT_RESERVED] != 0x00) or (rxpacket[PKT_ID] > 0xFC) or ( + DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN) or ( + rxpacket[PKT_INSTRUCTION] != 0x55): + # remove the first byte in the packet + del rxpacket[0] + rx_length -= 1 + continue + + if wait_length != (DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1): + wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1 + continue + + if rx_length < wait_length: + if port.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + else: + continue + + crc = DXL_MAKEWORD(rxpacket[wait_length - 2], rxpacket[wait_length - 1]) + + if self.updateCRC(0, rxpacket, wait_length - 2) == crc: + result = COMM_SUCCESS + else: + result = COMM_RX_CORRUPT + break + + else: + # remove unnecessary packets + del rxpacket[0: idx] + rx_length -= idx + + else: + if port.isPacketTimeout(): + if rx_length == 0: + result = COMM_RX_TIMEOUT + else: + result = COMM_RX_CORRUPT + break + + port.is_using = False + + if result == COMM_SUCCESS: + rxpacket = self.removeStuffing(rxpacket) + + return rxpacket, result + + # NOT for BulkRead / SyncRead instruction + def txRxPacket(self, port, txpacket): + rxpacket = None + error = 0 + + # tx packet + result = self.txPacket(port, txpacket) + if result != COMM_SUCCESS: + return rxpacket, result, error + + # (Instruction == BulkRead or SyncRead) == this function is not available. + if txpacket[PKT_INSTRUCTION] == INST_BULK_READ or txpacket[PKT_INSTRUCTION] == INST_SYNC_READ: + result = COMM_NOT_AVAILABLE + + # (ID == Broadcast ID) == no need to wait for status packet or not available. + # (Instruction == action) == no need to wait for status packet + if txpacket[PKT_ID] == BROADCAST_ID or txpacket[PKT_INSTRUCTION] == INST_ACTION: + port.is_using = False + return rxpacket, result, error + + # set packet timeout + if txpacket[PKT_INSTRUCTION] == INST_READ: + port.setPacketTimeout(DXL_MAKEWORD(txpacket[PKT_PARAMETER0 + 2], txpacket[PKT_PARAMETER0 + 3]) + 11) + else: + port.setPacketTimeout(11) + # HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H + + # rx packet + while True: + rxpacket, result = self.rxPacket(port) + if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: + break + + if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: + error = rxpacket[PKT_ERROR] + + return rxpacket, result, error + + def ping(self, port, dxl_id): + model_number = 0 + error = 0 + + txpacket = [0] * 10 + + if dxl_id >= BROADCAST_ID: + return model_number, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = 3 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_PING + + rxpacket, result, error = self.txRxPacket(port, txpacket) + if result == COMM_SUCCESS: + model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]) + + return model_number, result, error + + def broadcastPing(self, port): + data_list = {} + + STATUS_LENGTH = 14 + + rx_length = 0 + wait_length = STATUS_LENGTH * MAX_ID + + txpacket = [0] * 10 + rxpacket = [] + + txpacket[PKT_ID] = BROADCAST_ID + txpacket[PKT_LENGTH_L] = 3 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_PING + + result = self.txPacket(port, txpacket) + if result != COMM_SUCCESS: + port.is_using = False + return data_list, result + + # set rx timeout + port.setPacketTimeout(wait_length * 1) + + while True: + rxpacket += port.readPort(wait_length - rx_length) + rx_length = len(rxpacket) + + if port.isPacketTimeout(): # or rx_length >= wait_length + break + + port.is_using = False + + if rx_length == 0: + return data_list, COMM_RX_TIMEOUT + + while True: + if rx_length < STATUS_LENGTH: + return data_list, COMM_RX_CORRUPT + + # find packet header + for idx in range(0, rx_length - 2): + if rxpacket[idx] == 0xFF and rxpacket[idx + 1] == 0xFF and rxpacket[idx + 2] == 0xFD: + break + + if idx == 0: # found at the beginning of the packet + # verify CRC16 + crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH - 2], rxpacket[STATUS_LENGTH - 1]) + + if self.updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc: + result = COMM_SUCCESS + + data_list[rxpacket[PKT_ID]] = [ + DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]), + rxpacket[PKT_PARAMETER0 + 3]] + + del rxpacket[0: STATUS_LENGTH] + rx_length = rx_length - STATUS_LENGTH + + if rx_length == 0: + return data_list, result + + else: + result = COMM_RX_CORRUPT + + # remove header (0xFF 0xFF 0xFD) + del rxpacket[0: 3] + rx_length = rx_length - 3 + + else: + # remove unnecessary packets + del rxpacket[0: idx] + rx_length = rx_length - idx + + # FIXME: unreachable code + return data_list, result + + def action(self, port, dxl_id): + txpacket = [0] * 10 + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = 3 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_ACTION + + _, result, _ = self.txRxPacket(port, txpacket) + return result + + def reboot(self, port, dxl_id): + txpacket = [0] * 10 + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = 3 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_REBOOT + + _, result, error = self.txRxPacket(port, txpacket) + return result, error + + def factoryReset(self, port, dxl_id, option): + txpacket = [0] * 11 + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = 4 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET + txpacket[PKT_PARAMETER0] = option + + _, result, error = self.txRxPacket(port, txpacket) + return result, error + + def readTx(self, port, dxl_id, address, length): + txpacket = [0] * 14 + + if dxl_id >= BROADCAST_ID: + return COMM_NOT_AVAILABLE + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = 7 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) + txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(length) + txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(length) + + result = self.txPacket(port, txpacket) + + # set packet timeout + if result == COMM_SUCCESS: + port.setPacketTimeout(length + 11) + + return result + + def readRx(self, port, dxl_id, length): + result = COMM_TX_FAIL + error = 0 + + rxpacket = None + data = [] + + while True: + rxpacket, result = self.rxPacket(port) + + if result != COMM_SUCCESS or rxpacket[PKT_ID] == dxl_id: + break + + if result == COMM_SUCCESS and rxpacket[PKT_ID] == dxl_id: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length]) + + return data, result, error + + def readTxRx(self, port, dxl_id, address, length): + error = 0 + + txpacket = [0] * 14 + data = [] + + if dxl_id >= BROADCAST_ID: + return data, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = 7 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_READ + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) + txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(length) + txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(length) + + rxpacket, result, error = self.txRxPacket(port, txpacket) + if result == COMM_SUCCESS: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length]) + + return data, result, error + + def read1ByteTx(self, port, dxl_id, address): + return self.readTx(port, dxl_id, address, 1) + + def read1ByteRx(self, port, dxl_id): + data, result, error = self.readRx(port, dxl_id, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read1ByteTxRx(self, port, dxl_id, address): + data, result, error = self.readTxRx(port, dxl_id, address, 1) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTx(self, port, dxl_id, address): + return self.readTx(port, dxl_id, address, 2) + + def read2ByteRx(self, port, dxl_id): + data, result, error = self.readRx(port, dxl_id, 2) + data_read = DXL_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read2ByteTxRx(self, port, dxl_id, address): + data, result, error = self.readTxRx(port, dxl_id, address, 2) + data_read = DXL_MAKEWORD(data[0], data[1]) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTx(self, port, dxl_id, address): + return self.readTx(port, dxl_id, address, 4) + + def read4ByteRx(self, port, dxl_id): + data, result, error = self.readRx(port, dxl_id, 4) + data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), + DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read4ByteTxRx(self, port, dxl_id, address): + data, result, error = self.readTxRx(port, dxl_id, address, 4) + data_read = DXL_MAKEDWORD(DXL_MAKEWORD(data[0], data[1]), + DXL_MAKEWORD(data[2], data[3])) if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def writeTxOnly(self, port, dxl_id, address, length, data): + txpacket = [0] * (length + 12) + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5) + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5) + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] + + result = self.txPacket(port, txpacket) + port.is_using = False + + return result + + def writeTxRx(self, port, dxl_id, address, length, data): + txpacket = [0] * (length + 12) + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5) + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5) + txpacket[PKT_INSTRUCTION] = INST_WRITE + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] + rxpacket, result, error = self.txRxPacket(port, txpacket) + + return result, error + + def write1ByteTxOnly(self, port, dxl_id, address, data): + data_write = [data] + return self.writeTxOnly(port, dxl_id, address, 1, data_write) + + def write1ByteTxRx(self, port, dxl_id, address, data): + data_write = [data] + return self.writeTxRx(port, dxl_id, address, 1, data_write) + + def write2ByteTxOnly(self, port, dxl_id, address, data): + data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] + return self.writeTxOnly(port, dxl_id, address, 2, data_write) + + def write2ByteTxRx(self, port, dxl_id, address, data): + data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] + return self.writeTxRx(port, dxl_id, address, 2, data_write) + + def write4ByteTxOnly(self, port, dxl_id, address, data): + data_write = [DXL_LOBYTE(DXL_LOWORD(data)), + DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), + DXL_HIBYTE(DXL_HIWORD(data))] + return self.writeTxOnly(port, dxl_id, address, 4, data_write) + + def write4ByteTxRx(self, port, dxl_id, address, data): + data_write = [DXL_LOBYTE(DXL_LOWORD(data)), + DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), + DXL_HIBYTE(DXL_HIWORD(data))] + return self.writeTxRx(port, dxl_id, address, 4, data_write) + + def regWriteTxOnly(self, port, dxl_id, address, length, data): + txpacket = [0] * (length + 12) + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5) + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5) + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] + + result = self.txPacket(port, txpacket) + port.is_using = False + + return result + + def regWriteTxRx(self, port, dxl_id, address, length, data): + txpacket = [0] * (length + 12) + + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5) + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5) + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) + + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] + + _, result, error = self.txRxPacket(port, txpacket) + + return result, error + + def syncReadTx(self, port, start_address, data_length, param, param_length): + txpacket = [0] * (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 + + 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 + txpacket[PKT_INSTRUCTION] = INST_SYNC_READ + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address) + txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length) + txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length) + + txpacket[PKT_PARAMETER0 + 4: PKT_PARAMETER0 + 4 + param_length] = param[0: param_length] + + result = self.txPacket(port, txpacket) + if result == COMM_SUCCESS: + port.setPacketTimeout((11 + data_length) * param_length) + + return result + + def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): + txpacket = [0] * (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 + + 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 + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address) + txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length) + txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length) + + txpacket[PKT_PARAMETER0 + 4: PKT_PARAMETER0 + 4 + param_length] = param[0: param_length] + + _, result, _ = self.txRxPacket(port, txpacket) + + return result + + def bulkReadTx(self, port, param, param_length): + txpacket = [0] * (param_length + 10) + # 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + + 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 + txpacket[PKT_INSTRUCTION] = INST_BULK_READ + + txpacket[PKT_PARAMETER0: PKT_PARAMETER0 + param_length] = param[0: param_length] + + result = self.txPacket(port, txpacket) + if result == COMM_SUCCESS: + wait_length = 0 + i = 0 + while i < param_length: + wait_length += DXL_MAKEWORD(param[i + 3], param[i + 4]) + 10 + i += 5 + port.setPacketTimeout(wait_length) + + return result + + def bulkWriteTxOnly(self, port, param, param_length): + txpacket = [0] * (param_length + 10) + # 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + + 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 + txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE + + txpacket[PKT_PARAMETER0: PKT_PARAMETER0 + param_length] = param[0: param_length] + + _, result, _ = self.txRxPacket(port, txpacket) + + return result diff --git a/python/src/dynamixel_sdk/robotis_def.py b/python/src/dynamixel_sdk/robotis_def.py new file mode 100644 index 00000000..aacadd76 --- /dev/null +++ b/python/src/dynamixel_sdk/robotis_def.py @@ -0,0 +1,74 @@ +#!/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) + +BROADCAST_ID = 0xFE # 254 +MAX_ID = 0xFC # 252 + +# Instruction for DXL Protocol +INST_PING = 1 +INST_READ = 2 +INST_WRITE = 3 +INST_REG_WRITE = 4 +INST_ACTION = 5 +INST_FACTORY_RESET = 6 +INST_SYNC_WRITE = 131 # 0x83 +INST_BULK_READ = 146 # 0x92 +# --- Only for 2.0 --- +INST_REBOOT = 8 +INST_STATUS = 85 # 0x55 +INST_SYNC_READ = 130 # 0x82 +INST_BULK_WRITE = 147 # 0x93 + +# Communication Result +COMM_SUCCESS = 0 # tx or rx packet communication success +COMM_PORT_BUSY = -1000 # Port is busy (in use) +COMM_TX_FAIL = -1001 # Failed transmit instruction packet +COMM_RX_FAIL = -1002 # Failed get status packet +COMM_TX_ERROR = -2000 # Incorrect instruction packet +COMM_RX_WAITING = -3000 # Now recieving status packet +COMM_RX_TIMEOUT = -3001 # There is no status packet +COMM_RX_CORRUPT = -3002 # Incorrect status packet +COMM_NOT_AVAILABLE = -9000 # + + +# Macro for Control Table Value +def DXL_MAKEWORD(a, b): + return (a & 0xFF) | ((b & 0xFF) << 8) + + +def DXL_MAKEDWORD(a, b): + return (a & 0xFFFF) | (b & 0xFFFF) << 16 + + +def DXL_LOWORD(l): + return l & 0xFFFF + + +def DXL_HIWORD(l): + return (l >> 16) & 0xFFFF + + +def DXL_LOBYTE(w): + return w & 0xFF + + +def DXL_HIBYTE(w): + return (w >> 8) & 0xFF diff --git a/python/tests/protocol1_0/bulk_read.py b/python/tests/protocol1_0/bulk_read.py new file mode 100644 index 00000000..9562ea12 --- /dev/null +++ b/python/tests/protocol1_0/bulk_read.py @@ -0,0 +1,216 @@ +#!/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) + +# +# ********* Bulk Read Example ********* +# +# +# Available Dynamixel model on this example : MX or X series set to Protocol 1.0 +# This example is tested with two Dynamixel MX-28, and an USB2DYNAMIXEL +# Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_MX_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +ADDR_MX_GOAL_POSITION = 116 +ADDR_MX_PRESENT_POSITION = 132 +ADDR_MX_MOVING = 122 + +# Data Byte Length +LEN_MX_GOAL_POSITION = 4 +LEN_MX_PRESENT_POSITION = 4 +LEN_MX_MOVING = 1 + +# Protocol version +PROTOCOL_VERSION = 1.0 # See which protocol version is used in the Dynamixel + +# Default setting +DXL1_ID = 1 # Dynamixel#1 ID : 1 +DXL2_ID = 2 # Dynamixel#1 ID : 2 +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 +DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position + +# 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) + +# Initialize GroupBulkRead instace for Present Position +groupBulkRead = GroupBulkRead(portHandler, packetHandler) + +# 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() + + +# Enable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_MX_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#%d has been successfully connected" % DXL1_ID) + +# Enable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_MX_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#%d has been successfully connected" % DXL2_ID) + +# Add parameter storage for Dynamixel#1 present position +dxl_addparam_result = groupBulkRead.addParam(DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION) +if dxl_addparam_result != True: + print("[ID:%03d] groupBulkRead addparam failed" % DXL1_ID) + quit() + +# Add parameter storage for Dynamixel#2 moving value +dxl_addparam_result = groupBulkRead.addParam(DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING) +if dxl_addparam_result != True: + print("[ID:%03d] groupBulkRead addparam failed" % DXL2_ID) + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write Dynamixel#1 goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL1_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: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + # Write Dynamixel#2 goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL2_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: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + while 1: + # Bulkread present position and moving status + dxl_comm_result = groupBulkRead.txRxPacket() + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + + # Check if groupbulkread data of Dynamixel#1 is available + dxl_getdata_result = groupBulkRead.isAvailable(DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION) + if dxl_getdata_result != True: + print("[ID:%03d] groupBulkRead getdata failed" % DXL1_ID) + quit() + + # Check if groupbulkread data of Dynamixel#2 is available + dxl_getdata_result = groupBulkRead.isAvailable(DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING) + if dxl_getdata_result != True: + print("[ID:%03d] groupBulkRead getdata failed" % DXL2_ID) + quit() + + # Get Dynamixel#1 present position value + dxl1_present_position = groupBulkRead.getData(DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION) + + # Get Dynamixel#2 moving value + dxl2_moving_value = groupBulkRead.getData(DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING) + + print("[ID:%03d] Present Position : %d \t [ID:%03d] Is Moving: %d" % (DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_moving_value)) + + if not (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Clear bulkread parameter storage +groupBulkRead.clearParam() + +# Disable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_MX_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)) + +# Disable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_MX_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/python/tests/protocol1_0/factory_reset.py b/python/tests/protocol1_0/factory_reset.py new file mode 100644 index 00000000..fb797f04 --- /dev/null +++ b/python/tests/protocol1_0/factory_reset.py @@ -0,0 +1,168 @@ +#!/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) + +# +# ********* Factory Reset Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 1.0 +# This example is tested with a Dynamixel MX-28, and an USB2DYNAMIXEL +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) +# + +# Be aware that: +# This example resets all properties of Dynamixel to default values, such as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) +# + + +import os +from time import sleep + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_MX_BAUDRATE = 8 # Control table address is different in Dynamixel model + +# Protocol version +PROTOCOL_VERSION = 1.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-*" + +FACTORYRST_DEFAULTBAUDRATE = 57600 # Dynamixel baudrate set by factoryreset +NEW_BAUDNUM = 1 # New baudnum to recover Dynamixel baudrate as it was +OPERATION_MODE = 0x00 # Mode is unavailable in Protocol 1.0 Reset + +# 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() + +# Read present baudrate of the controller +print("Now the controller baudrate is : %d" % portHandler.getBaudRate()) + +# Try factoryreset +print("[ID:%03d] Try factoryreset : " % DXL_ID) + +dxl_comm_result, dxl_error = packetHandler.factoryReset(portHandler, DXL_ID, OPERATION_MODE) +if dxl_comm_result != COMM_SUCCESS: + print("Aborted") + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + quit() +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + +# Wait for reset +print("Wait for reset...") +sleep(2.0) +print("[ID:%03d] factoryReset Success!" % DXL_ID) + +# Set controller baudrate to Dynamixel default baudrate +if portHandler.setBaudRate(FACTORYRST_DEFAULTBAUDRATE): + print("Succeeded to change the controller baudrate to : %d" % FACTORYRST_DEFAULTBAUDRATE) +else: + print("Failed to change the controller baudrate") + print("Press any key to terminate...") + quit() + + +# Read Dynamixel baudnum +dxl_baudnum_read, dxl_comm_result, dxl_error = packetHandler.read1ByteTxRx(portHandler, DXL_ID, ADDR_MX_BAUDRATE) +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("[ID:%03d] DXL baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) + +# Write new baudnum +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_BAUDRATE, NEW_BAUDNUM) +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("[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM)) + +# Set port baudrate to BAUDRATE +if portHandler.setBaudRate(BAUDRATE): + print("Succeeded to change the controller baudrate to : %d" % BAUDRATE) +else: + print("Failed to change the controller baudrate") + print("Press any key to terminate...") + quit() + +sleep(0.2) + +# Read Dynamixel baudnum +dxl_baudnum_read, dxl_comm_result, dxl_error = packetHandler.read1ByteTxRx(portHandler, DXL_ID, ADDR_MX_BAUDRATE) +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("[ID:%03d] Dynamixel Baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol1_0/multi_port.py b/python/tests/protocol1_0/multi_port.py new file mode 100644 index 00000000..e89c795b --- /dev/null +++ b/python/tests/protocol1_0/multi_port.py @@ -0,0 +1,206 @@ +#!/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) + +# +# ********* Multi Port Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 1.0 +# This example is tested with a Dynamixel MX-28, and two USB2DYNAMIXEL +# Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_MX_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +ADDR_MX_GOAL_POSITION = 116 +ADDR_MX_PRESENT_POSITION = 132 + +# Protocol version +PROTOCOL_VERSION = 1.0 # See which protocol version is used in the Dynamixel + +# Default setting +DXL1_ID = 1 # Dynamixel#1 ID : 1 +DXL2_ID = 2 # Dynamixel#2 ID : 2 +BAUDRATE = 57600 # Dynamixel default baudrate : 57600 +DEVICENAME1 = '/dev/ttyUSB0' # Check which port is being used on your controller +DEVICENAME2 = '/dev/ttyUSB1' # 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 +DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position + + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler1 = PortHandler(DEVICENAME1) +portHandler2 = PortHandler(DEVICENAME2) + +# Initialize PacketHandler instance +# Set the protocol version +# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler +packetHandler = PacketHandler(PROTOCOL_VERSION) + +# Open port1 +if portHandler1.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + quit() + +# Open port2 +if portHandler2.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + quit() + +# Set port1 baudrate +if portHandler1.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") +else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + getch() + quit() + +# Set port2 baudrate +if portHandler2.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") +else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + getch() + quit() + +# Enable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler1, DXL1_ID, ADDR_MX_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#%d has been successfully connected" % DXL1_ID) + +# Enable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler2, DXL2_ID, ADDR_MX_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#%d has been successfully connected" % DXL2_ID) + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write Dynamixel#1 goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler1, DXL1_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: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + # Write Dynamixel#2 goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler2, DXL2_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: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + + while 1: + # Read Dynamixel#1 present position + dxl1_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler1, DXL1_ID, ADDR_MX_PRESENT_POSITION) + if dxl_comm_result != COMM_SUCCESS: + print packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + + # Read Dynamixel#2 present position + dxl2_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler2, DXL2_ID, ADDR_MX_PRESENT_POSITION) + if dxl_comm_result != COMM_SUCCESS: + print packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + + print("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position)) + + if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) and (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + + +# Disable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler1, DXL1_ID, ADDR_MX_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)) + +# Disable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler2, DXL2_ID, ADDR_MX_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 port1 +portHandler1.closePort() + +# Close port2 +portHandler2.closePort() \ No newline at end of file diff --git a/python/protocol1_0/ping.py b/python/tests/protocol1_0/ping.py similarity index 50% rename from python/protocol1_0/ping.py rename to python/tests/protocol1_0/ping.py index 75f5c397..12d08c18 100644 --- a/python/protocol1_0/ping.py +++ b/python/tests/protocol1_0/ping.py @@ -20,12 +20,11 @@ # Author: Ryu Woon Jung (Leon) # -# ********* ping Example ********* +# ********* Ping Example ********* # # # Available Dynamixel model on this example : All models using Protocol 1.0 -# This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL. -# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +# This example is tested with a Dynamixel MX-28, and an USB2DYNAMIXEL # Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) # @@ -47,62 +46,55 @@ def getch(): termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library +from dynamixel_sdk import * # Uses Dynamixel SDK library # Protocol version -PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel +PROTOCOL_VERSION = 1.0 # See which protocol version is used in the Dynamixel # Default setting -DXL_ID = 1 # Dynamixel ID: 1 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" - -COMM_SUCCESS = 0 # Communication Success result value -COMM_TX_FAIL = -1001 # Communication Tx Failed +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-*" -# Initialize PortHandler Structs +# Initialize PortHandler instance # Set the port path # Get methods and members of PortHandlerLinux or PortHandlerWindows -port_num = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() +portHandler = PortHandler(DEVICENAME) -dxl_comm_result = COMM_TX_FAIL # Communication result +# Initialize PacketHandler instance +# Set the protocol version +# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler +packetHandler = PacketHandler(PROTOCOL_VERSION) # Open port -if dynamixel.openPort(port_num): - print("Succeeded to open the port!") +if portHandler.openPort(): + print("Succeeded to open the port") else: - print("Failed to open the port!") + print("Failed to open the port") print("Press any key to terminate...") getch() quit() + # Set port baudrate -if dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") +if portHandler.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") else: - print("Failed to change the baudrate!") + print("Failed to change the baudrate") print("Press any key to terminate...") getch() quit() - # Try to ping the Dynamixel # Get Dynamixel model number -dxl_model_number = dynamixel.pingGetModelNum(port_num, PROTOCOL_VERSION, DXL_ID) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) +dxl_model_number, dxl_comm_result, dxl_error = packetHandler.ping(portHandler, DXL_ID) if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: print("[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number)) # Close port -dynamixel.closePort(port_num) +portHandler.closePort() diff --git a/python/tests/protocol1_0/read_write.py b/python/tests/protocol1_0/read_write.py new file mode 100644 index 00000000..2d8fa88e --- /dev/null +++ b/python/tests/protocol1_0/read_write.py @@ -0,0 +1,153 @@ +#!/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) + +# +# ********* Read and Write Example ********* +# +# +# Available DXL model on this example : All models using Protocol 1.0 +# This example is tested with a DXL MX-28, and an USB2DYNAMIXEL +# Be sure that DXL MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_MX_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +ADDR_MX_GOAL_POSITION = 116 +ADDR_MX_PRESENT_POSITION = 132 + +# Protocol version +PROTOCOL_VERSION = 1.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 +DXL_MINIMUM_POSITION_VALUE = 10 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position + + +# 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() + +# Enable Dynamixel Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_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("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(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: + 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_MX_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, dxl_goal_position[index], dxl_present_position)) + + if not abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD: + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + + +# Disable Dynamixel Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_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/python/tests/protocol1_0/sync_write.py b/python/tests/protocol1_0/sync_write.py new file mode 100644 index 00000000..46a571c8 --- /dev/null +++ b/python/tests/protocol1_0/sync_write.py @@ -0,0 +1,201 @@ +#!/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) + +# +# ********* Sync Write Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 1.0 +# This example is tested with two Dynamixel MX-28, and an USB2DYNAMIXEL +# Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_MX_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +ADDR_MX_GOAL_POSITION = 116 +ADDR_MX_PRESENT_POSITION = 132 + +# Data Byte Length +LEN_MX_GOAL_POSITION = 4 +LEN_MX_PRESENT_POSITION = 4 + +# Protocol version +PROTOCOL_VERSION = 1.0 # See which protocol version is used in the Dynamixel + +# Default setting +DXL1_ID = 1 # Dynamixel#1 ID : 1 +DXL2_ID = 2 # Dynamixel#1 ID : 2 +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 +DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position + + +# 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) + +# Initialize GroupSyncWrite instance +groupSyncWrite = GroupSyncWrite(portHandler, packetHandler, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION) + +# 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() + + +# Enable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_MX_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#%d has been successfully connected" % DXL1_ID) + +# Enable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_MX_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#%d has been successfully connected" % DXL2_ID) + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Allocate goal position value into byte array + param_goal_position = [DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]))] + + # Add Dynamixel#1 goal position value to the Syncwrite parameter storage + dxl_addparam_result = groupSyncWrite.addParam(DXL1_ID, param_goal_position) + if dxl_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % DXL1_ID) + quit() + + # Add Dynamixel#2 goal position value to the Syncwrite parameter storage + dxl_addparam_result = groupSyncWrite.addParam(DXL2_ID, param_goal_position) + if dxl_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % DXL2_ID) + quit() + + # Syncwrite goal position + dxl_comm_result = groupSyncWrite.txPacket() + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + + # Clear syncwrite parameter storage + groupSyncWrite.clearParam() + + while 1: + # Read Dynamixel#1 present position + dxl1_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL1_ID, ADDR_MX_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)) + + # Read Dynamixel#2 present position + dxl2_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL2_ID, ADDR_MX_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\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position)) + + if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) and (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + + +# Disable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_MX_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)) + +# Disable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_MX_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/python/tests/protocol2_0/broadcast_ping.py b/python/tests/protocol2_0/broadcast_ping.py new file mode 100644 index 00000000..6e02c527 --- /dev/null +++ b/python/tests/protocol2_0/broadcast_ping.py @@ -0,0 +1,98 @@ +#!/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) + +# +# ********* broadcastPing Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 2.0 +# This example is tested with two Dynamixel PRO 54-200, and an USB2DYNAMIXEL +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Protocol version +PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel + +# Default setting +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-*" + +# 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() + +# Try to broadcast ping the Dynamixel +dxl_data_list, dxl_comm_result = packetHandler.broadcastPing(portHandler) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + +print("Detected Dynamixel :") +for dxl_id in dxl_data_list: + print("[ID:%03d] model version : %d | firmware version : %d" % (dxl_id, dxl_data_list.get(dxl_id)[0], dxl_data_list.get(dxl_id)[1])) + +# Close port +portHandler.closePort() diff --git a/python/tests/protocol2_0/bulk_read_write.py b/python/tests/protocol2_0/bulk_read_write.py new file mode 100644 index 00000000..651f32fb --- /dev/null +++ b/python/tests/protocol2_0/bulk_read_write.py @@ -0,0 +1,229 @@ +#!/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) + +# +# ********* Bulk Read and Bulk Write Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 2.0 +# This example is tested with two Dynamixel PRO 54-200, and an USB2DYNAMIXEL +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 and 2 / Baudnum : 1 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_PRO_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +ADDR_PRO_LED_RED = 65 +ADDR_PRO_GOAL_POSITION = 116 +ADDR_PRO_PRESENT_POSITION = 132 + +# Data Byte Length +LEN_PRO_LED_RED = 1 +LEN_PRO_GOAL_POSITION = 4 +LEN_PRO_PRESENT_POSITION = 4 + +# Protocol version +PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel + +# Default setting +DXL1_ID = 1 # Dynamixel#1 ID : 1 +DXL2_ID = 2 # Dynamixel#1 ID : 2 +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 +DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position +dxl_led_value = [0x00, 0x01] # Dynamixel LED value for write + +# 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) + +# Initialize GroupBulkWrite instance +groupBulkWrite = GroupBulkWrite(portHandler, packetHandler) + +# Initialize GroupBulkRead instace for Present Position +groupBulkRead = GroupBulkRead(portHandler, packetHandler) + +# 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() + + +# Enable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_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#%d has been successfully connected" % DXL1_ID) + +# Enable Dynamixel#1 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)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +else: + print("Dynamixel#%d has been successfully connected" % DXL2_ID) + +# Add parameter storage for Dynamixel#1 present position +dxl_addparam_result = groupBulkRead.addParam(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) +if dxl_addparam_result != True: + print("[ID:%03d] groupBulkRead addparam failed" % DXL1_ID) + quit() + +# Add parameter storage for Dynamixel#2 LED value +dxl_addparam_result = groupBulkRead.addParam(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED) +if dxl_addparam_result != True: + print("[ID:%03d] groupBulkRead addparam failed" % DXL2_ID) + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Allocate goal position value into byte array + param_goal_position = [DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]))] + + # Add Dynamixel#1 goal position value to the Bulkwrite parameter storage + dxl_addparam_result = groupBulkWrite.addParam(DXL1_ID, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION, param_goal_position) + if dxl_addparam_result != True: + print("[ID:%03d] groupBulkWrite addparam failed" % DXL1_ID) + quit() + + # Add Dynamixel#2 LED value to the Bulkwrite parameter storage + dxl_addparam_result = groupBulkWrite.addParam(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED, [dxl_led_value[index]]) + if dxl_addparam_result != True: + print("[ID:%03d] groupBulkWrite addparam failed" % DXL2_ID) + quit() + + # Bulkwrite goal position and LED value + dxl_comm_result = groupBulkWrite.txPacket() + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + + # Clear bulkwrite parameter storage + groupBulkWrite.clearParam() + + while 1: + # Bulkread present position and LED status + dxl_comm_result = groupBulkRead.txRxPacket() + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + + # Check if groupbulkread data of Dynamixel#1 is available + dxl_getdata_result = groupBulkRead.isAvailable(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) + if dxl_getdata_result != True: + print("[ID:%03d] groupBulkRead getdata failed" % DXL1_ID) + quit() + + # Check if groupbulkread data of Dynamixel#2 is available + dxl_getdata_result = groupBulkRead.isAvailable(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED) + if dxl_getdata_result != True: + print("[ID:%03d] groupBulkRead getdata failed" % DXL2_ID) + quit() + + # Get present position value + dxl1_present_position = groupBulkRead.getData(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) + + # Get LED value + dxl2_led_value_read = groupBulkRead.getData(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED) + + print("[ID:%03d] Present Position : %d \t [ID:%03d] LED Value: %d" % (DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_led_value_read)) + + if not (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Clear bulkread parameter storage +groupBulkRead.clearParam() + +# Disable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_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)) + +# Disable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_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/python/tests/protocol2_0/factory_reset.py b/python/tests/protocol2_0/factory_reset.py new file mode 100644 index 00000000..2ca496fe --- /dev/null +++ b/python/tests/protocol2_0/factory_reset.py @@ -0,0 +1,170 @@ +#!/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) + +# +# ********* Factory Reset Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 2.0 +# This example is tested with a Dynamixel PRO 54-200, and an USB2DYNAMIXEL +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + +# Be aware that: +# This example resets all properties of Dynamixel to default values, such as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + + +import os +from time import sleep + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_PRO_BAUDRATE = 8 # Control table address is different in Dynamixel model + +# 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-*" + +FACTORYRST_DEFAULTBAUDRATE = 57600 # Dynamixel baudrate set by factoryreset +NEW_BAUDNUM = 1 # New baudnum to recover Dynamixel baudrate as it was +OPERATION_MODE = 0x01 # 0xFF : reset all values + # 0x01 : reset all values except ID + # 0x02 : reset all values except ID and baudrate + +# 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() + +# Read present baudrate of the controller +print("Now the controller baudrate is : %d" % portHandler.getBaudRate()) + +# Try factoryreset +print("[ID:%03d] Try factoryreset : " % DXL_ID) + +dxl_comm_result, dxl_error = packetHandler.factoryReset(portHandler, DXL_ID, OPERATION_MODE) +if dxl_comm_result != COMM_SUCCESS: + print("Aborted") + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + quit() +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + +# Wait for reset +print("Wait for reset...") +sleep(2.0) +print("[ID:%03d] factoryReset Success!" % DXL_ID) + +# Set controller baudrate to Dynamixel default baudrate +if portHandler.setBaudRate(FACTORYRST_DEFAULTBAUDRATE): + print("Succeeded to change the controller baudrate to : %d" % FACTORYRST_DEFAULTBAUDRATE) +else: + print("Failed to change the controller baudrate") + print("Press any key to terminate...") + quit() + + +# Read Dynamixel baudnum +dxl_baudnum_read, dxl_comm_result, dxl_error = packetHandler.read1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_BAUDRATE) +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("[ID:%03d] DXL baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) + +# Write new baudnum +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_BAUDRATE, NEW_BAUDNUM) +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("[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM)) + +# Set port baudrate to BAUDRATE +if portHandler.setBaudRate(BAUDRATE): + print("Succeeded to change the controller baudrate to : %d" % BAUDRATE) +else: + print("Failed to change the controller baudrate") + print("Press any key to terminate...") + quit() + +sleep(0.2) + +# Read Dynamixel baudnum +dxl_baudnum_read, dxl_comm_result, dxl_error = packetHandler.read1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_BAUDRATE) +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("[ID:%03d] Dynamixel Baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol2_0/indirect_address.py b/python/tests/protocol2_0/indirect_address.py new file mode 100644 index 00000000..52013007 --- /dev/null +++ b/python/tests/protocol2_0/indirect_address.py @@ -0,0 +1,274 @@ +#!/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) + +# +# ********* Indirect Address Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 2.0 +# This example is tested with a Dynamixel PRO 54-200, and an USB2DYNAMIXEL +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +# Control table address is different in Dynamixel model +ADDR_PRO_INDIRECTADDRESS_FOR_WRITE = 168#49 # EEPROM region +ADDR_PRO_INDIRECTADDRESS_FOR_READ = 178#59 # EEPROM region +ADDR_PRO_TORQUE_ENABLE = 64#562 +ADDR_PRO_LED_RED = 65#563 +ADDR_PRO_GOAL_POSITION = 116#596 +ADDR_PRO_MOVING = 122#610 +ADDR_PRO_PRESENT_POSITION = 132#611 +ADDR_PRO_INDIRECTDATA_FOR_WRITE = 224#634 +ADDR_PRO_INDIRECTDATA_FOR_READ = 229#639 + +# Data Byte Length +LEN_PRO_LED_RED = 1 +LEN_PRO_GOAL_POSITION = 4 +LEN_PRO_MOVING = 1 +LEN_PRO_PRESENT_POSITION = 4 +LEN_PRO_INDIRECTDATA_FOR_WRITE = 5 +LEN_PRO_INDIRECTDATA_FOR_READ = 5 + +# 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 +DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MINIMUM_LED_VALUE = 0 # Dynamixel LED will light between this value +DXL_MAXIMUM_LED_VALUE = 1 # and this value +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position +dxl_led_value = [DXL_MINIMUM_LED_VALUE, DXL_MAXIMUM_LED_VALUE] # Dynamixel LED value + +# 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) + +# Initialize GroupSyncWrite instance +groupSyncWrite = GroupSyncWrite(portHandler, packetHandler, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE) + +# Initialize GroupSyncRead instace for Present Position +groupSyncRead = GroupSyncRead(portHandler, packetHandler, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ) + +# 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() + + +# Disable Dynamixel Torque : +# Indirect address would not accessible when the torque is already enabled +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_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)) +else: + print("[ID:%03d] Dynamixel has been successfully connected" % DXL_ID) + +# INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + +# Enable Dynamixel Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_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)) + + +# Add parameter storage for multiple values +dxl_addparam_result = groupSyncRead.addParam(DXL_ID) +if dxl_addparam_result != True: + print("[ID:%03d] groupSyncRead addparam failed" % DXL_ID) + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Allocate goal position value into byte array + param_indirect_data_for_write = [DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]))] + param_indirect_data_for_write.append(dxl_led_value[index]) + + # Add values to the Syncwrite parameter storage + dxl_addparam_result = groupSyncWrite.addParam(DXL_ID, param_indirect_data_for_write) + if dxl_addparam_result != True: + print("[ID:%03d]groupSyncWrite addparam failed" % DXL_ID) + quit() + + # Syncwrite all + dxl_comm_result = groupSyncWrite.txPacket() + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + + # Clear syncwrite parameter storage + groupSyncWrite.clearParam() + + while 1: + # Syncread present position from indirectdata2 + dxl_comm_result = groupSyncRead.txRxPacket() + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + + # Check if groupsyncread data of Dynamixel present position value is available + dxl_getdata_result = groupSyncRead.isAvailable(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION) + if dxl_getdata_result != True: + print("[ID:%03d] groupSyncRead getdata failed" % DXL_ID) + quit() + + # Check if groupsyncread data of Dynamixel moving status is available + dxl_getdata_result = groupSyncRead.isAvailable(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING) + if dxl_getdata_result != True: + print("[ID:%03d] groupSyncRead getdata failed" % DXL_ID) + quit() + + # Get Dynamixel present position value + dxl_present_position = groupSyncRead.getData(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION) + + # Get Dynamixel moving status value + dxl_moving = groupSyncRead.getData(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING) + + print("[ID:%03d] GoalPos:%d PresPos:%d IsMoving:%d" % (DXL_ID, dxl_goal_position[index], dxl_present_position, dxl_moving)) + + if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Clear syncread parameter storage +groupSyncRead.clearParam() + +# Disable Dynamixel Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_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/python/tests/protocol2_0/multi_port.py b/python/tests/protocol2_0/multi_port.py new file mode 100644 index 00000000..3a4346bd --- /dev/null +++ b/python/tests/protocol2_0/multi_port.py @@ -0,0 +1,206 @@ +#!/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) + +# +# ********* Multi Port Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 2.0 +# This example is tested with two Dynamixel PRO 54-200, and two USB2DYNAMIXEL +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_PRO_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +ADDR_PRO_GOAL_POSITION = 116 +ADDR_PRO_PRESENT_POSITION = 132 + +# Protocol version +PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel + +# Default setting +DXL1_ID = 1 # Dynamixel#1 ID : 1 +DXL2_ID = 2 # Dynamixel#2 ID : 2 +BAUDRATE = 57600 # Dynamixel default baudrate : 57600 +DEVICENAME1 = '/dev/ttyUSB0' # Check which port is being used on your controller +DEVICENAME2 = '/dev/ttyUSB1' # 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 +DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position + + +# Initialize PortHandler instance +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +portHandler1 = PortHandler(DEVICENAME1) +portHandler2 = PortHandler(DEVICENAME2) + +# Initialize PacketHandler instance +# Set the protocol version +# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler +packetHandler = PacketHandler(PROTOCOL_VERSION) + +# Open port1 +if portHandler1.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + quit() + +# Open port2 +if portHandler2.openPort(): + print("Succeeded to open the port") +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + quit() + +# Set port1 baudrate +if portHandler1.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") +else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + getch() + quit() + +# Set port2 baudrate +if portHandler2.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") +else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + getch() + quit() + +# Enable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler1, DXL1_ID, ADDR_PRO_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#%d has been successfully connected" % DXL1_ID) + +# Enable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler2, DXL2_ID, ADDR_PRO_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#%d has been successfully connected" % DXL2_ID) + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write Dynamixel#1 goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler1, DXL1_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index]) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + + # Write Dynamixel#2 goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler2, DXL2_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index]) + 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 Dynamixel#1 present position + dxl1_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler1, DXL1_ID, ADDR_PRO_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)) + + # Read Dynamixel#2 present position + dxl2_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler2, DXL2_ID, ADDR_PRO_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\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position)) + + if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) and (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + + +# Disable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler1, DXL1_ID, ADDR_PRO_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)) + +# Disable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler2, DXL2_ID, ADDR_PRO_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 port1 +portHandler1.closePort() + +# Close port2 +portHandler2.closePort() \ No newline at end of file diff --git a/python/protocol2_0/ping.py b/python/tests/protocol2_0/ping.py similarity index 56% rename from python/protocol2_0/ping.py rename to python/tests/protocol2_0/ping.py index 333ba7a2..8825d5c9 100644 --- a/python/protocol2_0/ping.py +++ b/python/tests/protocol2_0/ping.py @@ -47,62 +47,55 @@ def getch(): termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library +from dynamixel_sdk import * # Uses Dynamixel SDK library # Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel +PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel # Default setting -DXL_ID = 1 # Dynamixel ID: 1 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" - -COMM_SUCCESS = 0 # Communication Success result value -COMM_TX_FAIL = -1001 # Communication Tx Failed +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-*" -# Initialize PortHandler Structs +# Initialize PortHandler instance # Set the port path # Get methods and members of PortHandlerLinux or PortHandlerWindows -port_num = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() +portHandler = PortHandler(DEVICENAME) -dxl_comm_result = COMM_TX_FAIL # Communication result +# Initialize PacketHandler instance +# Set the protocol version +# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler +packetHandler = PacketHandler(PROTOCOL_VERSION) # Open port -if dynamixel.openPort(port_num): - print("Succeeded to open the port!") +if portHandler.openPort(): + print("Succeeded to open the port") else: - print("Failed to open the port!") + print("Failed to open the port") print("Press any key to terminate...") getch() quit() + # Set port baudrate -if dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") +if portHandler.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") else: - print("Failed to change the baudrate!") + print("Failed to change the baudrate") print("Press any key to terminate...") getch() quit() - # Try to ping the Dynamixel # Get Dynamixel model number -dxl_model_number = dynamixel.pingGetModelNum(port_num, PROTOCOL_VERSION, DXL_ID) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) +dxl_model_number, dxl_comm_result, dxl_error = packetHandler.ping(portHandler, DXL_ID) if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: print("[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number)) # Close port -dynamixel.closePort(port_num) +portHandler.closePort() diff --git a/python/tests/protocol2_0/read_write.py b/python/tests/protocol2_0/read_write.py new file mode 100644 index 00000000..355bbc1d --- /dev/null +++ b/python/tests/protocol2_0/read_write.py @@ -0,0 +1,154 @@ +#!/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) + +# +# ********* Read and Write 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(support.robotis.com) and edit below variables yourself. +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_PRO_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +ADDR_PRO_GOAL_POSITION = 116 +ADDR_PRO_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 +DXL_MINIMUM_POSITION_VALUE = 10 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position + + +# 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() + +# Enable Dynamixel Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_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("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index]) + 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_PRO_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, dxl_goal_position[index], dxl_present_position)) + + if not abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD: + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + + +# Disable Dynamixel Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_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/python/protocol2_0/rebooting.py b/python/tests/protocol2_0/reboot.py similarity index 51% rename from python/protocol2_0/rebooting.py rename to python/tests/protocol2_0/reboot.py index 4e809222..70296e0f 100644 --- a/python/protocol2_0/rebooting.py +++ b/python/tests/protocol2_0/reboot.py @@ -24,8 +24,7 @@ # # # 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(support.robotis.com) and edit below variables yourself. +# This example is tested with a Dynamixel PRO 54-200, and USB2DYNAMIXEL # Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) # @@ -47,46 +46,42 @@ def getch(): termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch -os.sys.path.append('../dynamixel_functions_py') # Path setting - -import dynamixel_functions as dynamixel # Uses Dynamixel SDK library +from dynamixel_sdk import * # Uses Dynamixel SDK library # Protocol version -PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel +PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel # Default setting -DXL_ID = 1 # Dynamixel ID: 1 -BAUDRATE = 57600 -DEVICENAME = "/dev/ttyUSB0".encode('utf-8')# Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" - -COMM_SUCCESS = 0 # Communication Success result value -COMM_TX_FAIL = -1001 # Communication Tx Failed +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-*" -# Initialize PortHandler Structs +# Initialize PortHandler instance # Set the port path # Get methods and members of PortHandlerLinux or PortHandlerWindows -port_num = dynamixel.portHandler(DEVICENAME) - -# Initialize PacketHandler Structs -dynamixel.packetHandler() +portHandler = PortHandler(DEVICENAME) -dxl_comm_result = COMM_TX_FAIL # Communication result +# Initialize PacketHandler instance +# Set the protocol version +# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler +packetHandler = PacketHandler(PROTOCOL_VERSION) # Open port -if dynamixel.openPort(port_num): - print("Succeeded to open the port!") +if portHandler.openPort(): + print("Succeeded to open the port") else: - print("Failed to open the port!") + print("Failed to open the port") print("Press any key to terminate...") getch() quit() + # Set port baudrate -if dynamixel.setBaudRate(port_num, BAUDRATE): - print("Succeeded to change the baudrate!") +if portHandler.setBaudRate(BAUDRATE): + print("Succeeded to change the baudrate") else: - print("Failed to change the baudrate!") + print("Failed to change the baudrate") print("Press any key to terminate...") getch() quit() @@ -98,15 +93,14 @@ def getch(): print("See the Dynamixel LED flickering") # Try reboot # Dynamixel LED will flicker while it reboots -dynamixel.reboot(port_num, PROTOCOL_VERSION, DXL_ID) -dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) -dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) +dxl_comm_result, dxl_error = packetHandler.reboot(portHandler, DXL_ID) if dxl_comm_result != COMM_SUCCESS: - print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) + print("%s" % packetHandler.getRxPacketError(dxl_error)) + +print("[ID:%03d] reboot Succeeded\n" % DXL_ID) -print("[ID:%03d] reboot Succeeded" % (DXL_ID)) # Close port -dynamixel.closePort(port_num) +portHandler.closePort() diff --git a/python/tests/protocol2_0/sync_read_write.py b/python/tests/protocol2_0/sync_read_write.py new file mode 100644 index 00000000..3636162e --- /dev/null +++ b/python/tests/protocol2_0/sync_read_write.py @@ -0,0 +1,227 @@ +#!/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) + +# +# ********* Sync Read and Sync Write Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 2.0 +# This example is tested with two Dynamixel PRO 54-200, and an USB2DYNAMIXEL +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_PRO_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +ADDR_PRO_GOAL_POSITION = 116 +ADDR_PRO_PRESENT_POSITION = 132 + +# Data Byte Length +LEN_PRO_GOAL_POSITION = 4 +LEN_PRO_PRESENT_POSITION = 4 + +# Protocol version +PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel + +# Default setting +DXL1_ID = 1 # Dynamixel#1 ID : 1 +DXL2_ID = 2 # Dynamixel#1 ID : 2 +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 +DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position + + +# 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) + +# Initialize GroupSyncWrite instance +groupSyncWrite = GroupSyncWrite(portHandler, packetHandler, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION) + +# Initialize GroupSyncRead instace for Present Position +groupSyncRead = GroupSyncRead(portHandler, packetHandler, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) + +# 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() + + +# Enable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_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#%d has been successfully connected" % DXL1_ID) + +# 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)) +elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) +else: + print("Dynamixel#%d has been successfully connected" % DXL2_ID) + +# Add parameter storage for Dynamixel#1 present position value +dxl_addparam_result = groupSyncRead.addParam(DXL1_ID) +if dxl_addparam_result != True: + print("[ID:%03d] groupSyncRead addparam failed" % DXL1_ID) + quit() + +# Add parameter storage for Dynamixel#2 present position value +dxl_addparam_result = groupSyncRead.addParam(DXL2_ID) +if dxl_addparam_result != True: + print("[ID:%03d] groupSyncRead addparam failed" % DXL2_ID) + quit() + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Allocate goal position value into byte array + param_goal_position = [DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]))] + + # Add Dynamixel#1 goal position value to the Syncwrite parameter storage + dxl_addparam_result = groupSyncWrite.addParam(DXL1_ID, param_goal_position) + if dxl_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % DXL1_ID) + quit() + + # Add Dynamixel#2 goal position value to the Syncwrite parameter storage + dxl_addparam_result = groupSyncWrite.addParam(DXL2_ID, param_goal_position) + if dxl_addparam_result != True: + print("[ID:%03d] groupSyncWrite addparam failed" % DXL2_ID) + quit() + + # Syncwrite goal position + dxl_comm_result = groupSyncWrite.txPacket() + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + + # Clear syncwrite parameter storage + groupSyncWrite.clearParam() + + while 1: + # Syncread present position + dxl_comm_result = groupSyncRead.txRxPacket() + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + + # Check if groupsyncread data of Dynamixel#1 is available + dxl_getdata_result = groupSyncRead.isAvailable(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) + if dxl_getdata_result != True: + print("[ID:%03d] groupSyncRead getdata failed" % DXL1_ID) + quit() + + # Check if groupsyncread data of Dynamixel#2 is available + dxl_getdata_result = groupSyncRead.isAvailable(DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) + if dxl_getdata_result != True: + print("[ID:%03d] groupSyncRead getdata failed" % DXL2_ID) + quit() + + # Get Dynamixel#1 present position value + dxl1_present_position = groupSyncRead.getData(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) + + # Get Dynamixel#2 present position value + dxl2_present_position = groupSyncRead.getData(DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION) + + print("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position)) + + if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) and (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + +# Clear syncread parameter storage +groupSyncRead.clearParam() + +# Disable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_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)) + +# Disable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_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/python/tests/protocol_combined.py b/python/tests/protocol_combined.py new file mode 100644 index 00000000..a8ead995 --- /dev/null +++ b/python/tests/protocol_combined.py @@ -0,0 +1,200 @@ +#!/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) + +# +# ********* Protocol Combined Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 1.0 and 2.0 +# This example is tested with a Dynamixel MX-28, a Dynamixel PRO 54-200 and an USB2DYNAMIXEL +# Be sure that properties of Dynamixel MX and PRO are already set as %% MX - ID : 1 / Baudnum : 34 (Baudrate : 57600) , PRO - ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + +# Be aware that: +# This example configures two different control tables (especially, if it uses Dynamixel and Dynamixel PRO). It may modify critical Dynamixel parameter on the control table, if Dynamixels have wrong ID. +# + +import os + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import sys, tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + def getch(): + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address for Dynamixel MX +ADDR_MX_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +ADDR_MX_GOAL_POSITION = 116 +ADDR_MX_PRESENT_POSITION = 132 + +# Control table address for Dynamixel PRO +ADDR_PRO_TORQUE_ENABLE = 64 +ADDR_PRO_GOAL_POSITION = 116 +ADDR_PRO_PRESENT_POSITION = 132 + +# Protocol version +PROTOCOL_VERSION1 = 1.0 # See which protocol version is used in the Dynamixel +PROTOCOL_VERSION2 = 2.0 + +# Default setting +DXL1_ID = 1 # Dynamixel#1 ID : 1 +DXL2_ID = 2 # Dynamixel#2 ID : 2 +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 +DXL1_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value +DXL1_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL2_MINIMUM_POSITION_VALUE = 100 +DXL2_MAXIMUM_POSITION_VALUE = 4000 +DXL1_MOVING_STATUS_THRESHOLD = 10 # Dynamixel MX moving status threshold +DXL2_MOVING_STATUS_THRESHOLD = 20 # Dynamixel PRO moving status threshold + +index = 0 +dxl1_goal_position = [DXL1_MINIMUM_POSITION_VALUE, DXL1_MAXIMUM_POSITION_VALUE] # Goal position of Dynamixel MX +dxl2_goal_position = [DXL2_MINIMUM_POSITION_VALUE, DXL2_MAXIMUM_POSITION_VALUE] # Goal position of Dynamixel PRO + +# 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 +packetHandler1 = PacketHandler(PROTOCOL_VERSION1) +packetHandler2 = PacketHandler(PROTOCOL_VERSION2) + + +# 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() + +# Enable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler1.write1ByteTxRx(portHandler, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler1.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler1.getRxPacketError(dxl_error)) +else: + print("Dynamixel#%d has been successfully connected" % DXL1_ID) + +# Enable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler2.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler2.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler2.getRxPacketError(dxl_error)) +else: + print("Dynamixel#%d has been successfully connected" % DXL2_ID) + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(0x1b): + break + + # Write Dynamixel#1 goal position + dxl_comm_result, dxl_error = packetHandler1.write4ByteTxRx(portHandler, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl1_goal_position[index]) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler1.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler1.getRxPacketError(dxl_error)) + + # Write Dynamixel#2 goal position + dxl_comm_result, dxl_error = packetHandler2.write4ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_GOAL_POSITION, dxl2_goal_position[index]) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler2.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler2.getRxPacketError(dxl_error)) + + + while 1: + # Read Dynamixel#1 present position + dxl1_present_position, dxl_comm_result, dxl_error = packetHandler1.read4ByteTxRx(portHandler, DXL1_ID, ADDR_MX_PRESENT_POSITION) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler1.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler1.getRxPacketError(dxl_error)) + + # Read Dynamixel#2 present position + dxl2_present_position, dxl_comm_result, dxl_error = packetHandler2.read4ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_PRESENT_POSITION) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler2.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler2.getRxPacketError(dxl_error)) + + print("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL1_ID, dxl1_goal_position[index], dxl1_present_position, DXL2_ID, dxl2_goal_position[index], dxl2_present_position)) + + if not ((abs(dxl1_goal_position[index] - dxl1_present_position) > DXL1_MOVING_STATUS_THRESHOLD) and (abs(dxl2_goal_position[index] - dxl2_present_position) > DXL2_MOVING_STATUS_THRESHOLD)): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + + +# Disable Dynamixel#1 Torque +dxl_comm_result, dxl_error = packetHandler1.write1ByteTxRx(portHandler, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler1.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler1.getRxPacketError(dxl_error)) + +# Disable Dynamixel#2 Torque +dxl_comm_result, dxl_error = packetHandler2.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) +if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler2.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print("%s" % packetHandler2.getRxPacketError(dxl_error)) + +# Close port +portHandler.closePort()