From ca39f241d1028e0bc760655d295336a51c08ac34 Mon Sep 17 00:00:00 2001 From: leon Date: Fri, 1 Dec 2017 14:14:03 +0900 Subject: [PATCH 01/25] NOTIFICATION - RULES IN CONTRIBUTION --- c++/CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/c++/CHANGELOG.rst b/c++/CHANGELOG.rst index 7e08a1e2..a61592f5 100644 --- a/c++/CHANGELOG.rst +++ b/c++/CHANGELOG.rst @@ -5,7 +5,7 @@ Changelog for package dynamixel_sdk 3.5.4 (2017-12-01) ----------- * Added : Deprecated is now being shown by attributes #67 #107 -* Fixes : DynamixelSDK ROS Indigo Issue - target_sources func in CMake +* Fixes : DynamixelSDK ROS Indigo Issue - target_sources func in the CMake * Fixes : Bug in protocol1_packet_handler.cpp, line 222 checking the returned Error Mask #120 * Fixes : Packet Handlers - array param uint8_t to uint16_t to avoid closure loop when the packet is too long to be in uint8_t appropriately * Fixes : Group Syncwrite using multiple ports in c library issue solved (test code is also in this issue bulletin) #124 From 3fbcc62a0ea7b3a80adfd5979d7a812f7b18ee00 Mon Sep 17 00:00:00 2001 From: leon Date: Fri, 1 Dec 2017 14:15:20 +0900 Subject: [PATCH 02/25] NOTIFICATION - RULES IN CONTRIBUTION --- CONTRIBUTING.md | 8 ++++---- c++/CHANGELOG.rst | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) 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/c++/CHANGELOG.rst b/c++/CHANGELOG.rst index a61592f5..7e08a1e2 100644 --- a/c++/CHANGELOG.rst +++ b/c++/CHANGELOG.rst @@ -5,7 +5,7 @@ Changelog for package dynamixel_sdk 3.5.4 (2017-12-01) ----------- * Added : Deprecated is now being shown by attributes #67 #107 -* Fixes : DynamixelSDK ROS Indigo Issue - target_sources func in the CMake +* Fixes : DynamixelSDK ROS Indigo Issue - target_sources func in CMake * Fixes : Bug in protocol1_packet_handler.cpp, line 222 checking the returned Error Mask #120 * Fixes : Packet Handlers - array param uint8_t to uint16_t to avoid closure loop when the packet is too long to be in uint8_t appropriately * Fixes : Group Syncwrite using multiple ports in c library issue solved (test code is also in this issue bulletin) #124 From a8db4144cb6ad7ad126f427902544dbe61b38c19 Mon Sep 17 00:00:00 2001 From: leon Date: Fri, 1 Dec 2017 14:40:57 +0900 Subject: [PATCH 03/25] Protocol2PacketHandler::regWriteTxRx uint8_t -> uint16_t --- c++/src/dynamixel_sdk/protocol2_packet_handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp index 2b35069c..8c0d7eac 100644 --- a/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/c++/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -711,7 +711,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 +914,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); From d2daf85a3b7356dcebec1cd1ea407d6c4c7170f7 Mon Sep 17 00:00:00 2001 From: leon Date: Thu, 7 Dec 2017 16:34:20 +0900 Subject: [PATCH 04/25] DynamixelSDK MATLAB 2017 - new typedef (int8_t / int16_t / int32_t) applied in robotis_def.h --- README.md | 4 ++-- ReleaseNote.txt | 10 ++++++++++ c++/CHANGELOG.rst | 6 ++++++ c++/library.properties | 2 +- c++/package.xml | 2 +- c/include/dynamixel_sdk/robotis_def.h | 6 +++--- 6 files changed, 23 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index e892fe87..16746d13 100644 --- a/README.md +++ b/README.md @@ -5,8 +5,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.5.5| +| |||(--.--.--)| | OS | Linux | Windows | Linux + Windows + Mac | | Available Dynamixel models | All models | All models | All models | ||||| diff --git a/ReleaseNote.txt b/ReleaseNote.txt index be4338fe..1703d3ed 100644 --- a/ReleaseNote.txt +++ b/ReleaseNote.txt @@ -1,3 +1,13 @@ +============================================== + Dynamixel SDK 3.5.5 (Protocol 1.0/2.0) +============================================== + +- --.--.---- + +* Added : CONTRIBUTING.md added +* Changes : ISSUE_TEMPLATE.md modified +* Fixes : DynamixelSDK MATLAB 2017 - new typedef (int8_t / int16_t / int32_t) applied in robotis_def.h + ============================================== Dynamixel SDK 3.5.4 (Protocol 1.0/2.0) ============================================== diff --git a/c++/CHANGELOG.rst b/c++/CHANGELOG.rst index 7e08a1e2..17bebbad 100644 --- a/c++/CHANGELOG.rst +++ b/c++/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package dynamixel_sdk ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.5.5 (****-**-**) +----------- +* Added : CONTRIBUTING.md added +* Changes : ISSUE_TEMPLATE.md modified +* Fixes : DynamixelSDK MATLAB 2017 - new typedef (int8_t / int16_t / int32_t) applied in robotis_def.h + 3.5.4 (2017-12-01) ----------- * Added : Deprecated is now being shown by attributes #67 #107 diff --git a/c++/library.properties b/c++/library.properties index 61c03a03..419f7bd9 100644 --- a/c++/library.properties +++ b/c++/library.properties @@ -1,5 +1,5 @@ name=DynamixelSDK -version=3.5.4 +version=3.5.5 author=ROBOTIS maintainer=ROBOTIS sentence=DynamixelSDK for Arduino diff --git a/c++/package.xml b/c++/package.xml index 8a74e8df..b06b39c1 100644 --- a/c++/package.xml +++ b/c++/package.xml @@ -1,7 +1,7 @@ dynamixel_sdk - 3.5.4 + 3.5.5 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/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; From 8f89889615c99086130431cafafe27e5a01b3762 Mon Sep 17 00:00:00 2001 From: Leon Ryuwoon Jung Date: Tue, 2 Jan 2018 22:28:15 +0900 Subject: [PATCH 05/25] * Changes : C++ version - SyncRead / BulkRead - getError functions added --- ReleaseNote.txt | 1 + c++/CHANGELOG.rst | 1 + .../protocol1.0/bulk_read/bulk_read.cpp | 13 +++++++++- .../bulk_read_write/bulk_read_write.cpp | 13 +++++++++- .../sync_read_write/sync_read_write.cpp | 13 +++++++++- c++/include/dynamixel_sdk/group_bulk_read.h | 11 ++++++++ c++/include/dynamixel_sdk/group_sync_read.h | 13 +++++++++- c++/src/dynamixel_sdk/group_bulk_read.cpp | 26 ++++++++++++++++++- c++/src/dynamixel_sdk/group_sync_read.cpp | 26 ++++++++++++++++++- 9 files changed, 111 insertions(+), 6 deletions(-) diff --git a/ReleaseNote.txt b/ReleaseNote.txt index 1703d3ed..d047714c 100644 --- a/ReleaseNote.txt +++ b/ReleaseNote.txt @@ -7,6 +7,7 @@ * Added : CONTRIBUTING.md added * Changes : ISSUE_TEMPLATE.md modified * Fixes : DynamixelSDK MATLAB 2017 - new typedef (int8_t / int16_t / int32_t) applied in robotis_def.h +* Changes : C++ version - SyncRead / BulkRead - getError functions added ============================================== Dynamixel SDK 3.5.4 (Protocol 1.0/2.0) diff --git a/c++/CHANGELOG.rst b/c++/CHANGELOG.rst index 17bebbad..5d196cc2 100644 --- a/c++/CHANGELOG.rst +++ b/c++/CHANGELOG.rst @@ -7,6 +7,7 @@ Changelog for package dynamixel_sdk * Added : CONTRIBUTING.md added * Changes : ISSUE_TEMPLATE.md modified * Fixes : DynamixelSDK MATLAB 2017 - new typedef (int8_t / int16_t / int32_t) applied in robotis_def.h +* Changes : C++ version - SyncRead / BulkRead - getError functions added 3.5.4 (2017-12-01) ----------- 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/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/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++/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 From ddabfaf70638dc186c5ef5da7694d2cb16a75090 Mon Sep 17 00:00:00 2001 From: Piyush Jaipuriyar Date: Sun, 4 Mar 2018 18:58:35 +0530 Subject: [PATCH 06/25] Added missing header file for reset and factory_reset example in c and c++: unistd.h was missing --- c++/example/protocol1.0/reset/reset.cpp | 1 + c++/example/protocol2.0/factory_reset/factory_reset.cpp | 1 + c/example/protocol1.0/reset/reset.c | 1 + c/example/protocol2.0/factory_reset/factory_reset.c | 1 + 4 files changed, 4 insertions(+) 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/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/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 From 66e4d43be1e77e2b676e175ba5e1f614c40e4000 Mon Sep 17 00:00:00 2001 From: leon Date: Thu, 26 Apr 2018 19:32:54 +0900 Subject: [PATCH 07/25] added: porting to Windows --- python/src/dynamixel_sdk/__init__.py | 18 ++- python/src/dynamixel_sdk/group_bulk_read.py | 32 ++--- python/src/dynamixel_sdk/group_bulk_write.py | 16 +-- python/src/dynamixel_sdk/group_sync_read.py | 16 +-- python/src/dynamixel_sdk/group_sync_write.py | 12 +- python/src/dynamixel_sdk/packet_handler.py | 8 +- python/src/dynamixel_sdk/port_handler.py | 38 ++++- .../src/dynamixel_sdk/port_handler_linux.py | 31 +--- .../src/dynamixel_sdk/port_handler_windows.py | 135 +++++++----------- .../dynamixel_sdk/protocol1_packet_handler.py | 46 +++--- .../dynamixel_sdk/protocol2_packet_handler.py | 74 +++++----- python/tests/protocol1_0/ping.py | 22 +-- python/tests/protocol1_0/read_write.py | 40 +++--- python/tests/protocol2_0/ping.py | 26 ++-- python/tests/protocol2_0/read_write.py | 4 +- 15 files changed, 249 insertions(+), 269 deletions(-) diff --git a/python/src/dynamixel_sdk/__init__.py b/python/src/dynamixel_sdk/__init__.py index b5d22c54..3005ec31 100644 --- a/python/src/dynamixel_sdk/__init__.py +++ b/python/src/dynamixel_sdk/__init__.py @@ -19,9 +19,15 @@ # 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 * \ No newline at end of file +# 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 * +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 index b1da0f5e..cc2082b6 100644 --- a/python/src/dynamixel_sdk/group_bulk_read.py +++ b/python/src/dynamixel_sdk/group_bulk_read.py @@ -19,7 +19,7 @@ # Author: Ryu Woon Jung (Leon) -from robotis_def import * +from .robotis_def import * PARAM_NUM_DATA = 0 PARAM_NUM_ADDRESS = 1 @@ -45,20 +45,20 @@ def makeParam(self): for id in self.data_dict: if self.ph.getProtocolVersion() == 1.0: - self.param.append(self.data_dict[id][2]) # LEN - self.param.append(id) # ID - self.param.append(self.data_dict[id][1]) # ADDR + self.param.append(self.data_dict[id][2]) # LEN + self.param.append(id) # ID + self.param.append(self.data_dict[id][1]) # ADDR else: - self.param.append(id) # ID - self.param.append(DXL_LOBYTE(self.data_dict[id][1])) # ADDR_L - self.param.append(DXL_HIBYTE(self.data_dict[id][1])) # ADDR_H - self.param.append(DXL_LOBYTE(self.data_dict[id][2])) # LEN_L - self.param.append(DXL_HIBYTE(self.data_dict[id][2])) # LEN_H + self.param.append(id) # ID + self.param.append(DXL_LOBYTE(self.data_dict[id][1])) # ADDR_L + self.param.append(DXL_HIBYTE(self.data_dict[id][1])) # ADDR_H + self.param.append(DXL_LOBYTE(self.data_dict[id][2])) # LEN_L + self.param.append(DXL_HIBYTE(self.data_dict[id][2])) # LEN_H def addParam(self, id, start_address, data_length): if id in self.data_dict: # id already exist return False - + data = [] # [0] * data_length self.data_dict[id] = [data, start_address, data_length] @@ -68,11 +68,11 @@ def addParam(self, id, start_address, data_length): def removeParam(self, id): if not id in self.data_dict: # NOT exist return - + del self.data_dict[id] self.is_param_changed = True - + def clearParam(self): self.data_dict.clear() return @@ -80,7 +80,7 @@ def clearParam(self): def txPacket(self): if len(self.data_dict.keys()) == 0: return COMM_NOT_AVAILABLE - + if self.is_param_changed == True or not self.param: self.makeParam() @@ -107,7 +107,7 @@ def rxPacket(self): return result - def txRxPacket(self): + def txRxPacket(self): result = COMM_TX_FAIL result = self.txPacket() @@ -126,7 +126,7 @@ def isAvailable(self, id, address, data_length): return False return True - + def getData(self, id, address, data_length): if self.isAvailable(id, address, data_length) == False: return 0 @@ -140,4 +140,4 @@ def getData(self, id, address, data_length): elif data_length == 4: return DXL_MAKEDWORD(DXL_MAKEWORD(self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 0], self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 1]), DXL_MAKEWORD(self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 2], self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 3])) else: - return 0 \ No newline at end of file + return 0 diff --git a/python/src/dynamixel_sdk/group_bulk_write.py b/python/src/dynamixel_sdk/group_bulk_write.py index a28e05c1..7b1ee0f9 100644 --- a/python/src/dynamixel_sdk/group_bulk_write.py +++ b/python/src/dynamixel_sdk/group_bulk_write.py @@ -19,7 +19,7 @@ # Author: Ryu Woon Jung (Leon) -from robotis_def import * +from .robotis_def import * class GroupBulkWrite: def __init__(self, port, ph): @@ -41,14 +41,14 @@ def makeParam(self): for id in self.data_list: if not self.data_list[id]: return - + self.param.append(id) self.param.append(DXL_LOBYTE(self.data_list[id][1])) self.param.append(DXL_HIBYTE(self.data_list[id][1])) self.param.append(DXL_LOBYTE(self.data_list[id][2])) self.param.append(DXL_HIBYTE(self.data_list[id][2])) - self.param.extend(self.data_list[id][0]) + self.param.extend(self.data_list[id][0]) def addParam(self, id, start_address, data_length, data): if self.ph.getProtocolVersion() == 1.0: @@ -56,7 +56,7 @@ def addParam(self, id, start_address, data_length, data): if id in self.data_list: # id already exist return False - + if len(data) > data_length: # input data is longer than set return False @@ -71,11 +71,11 @@ def removeParam(self, id): if not id in self.data_list: # NOT exist return - + del self.data_list[id] self.is_param_changed = True - + def changeParam(self, id, start_address, data_length, data): if self.ph.getProtocolVersion() == 1.0: return False @@ -101,8 +101,8 @@ def clearParam(self): 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 == True or len(self.param) == 0: self.makeParam() - return self.ph.bulkWriteTxOnly(self.port, self.param, len(self.param)) \ No newline at end of file + 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 index 758fdee2..9438dc12 100644 --- a/python/src/dynamixel_sdk/group_sync_read.py +++ b/python/src/dynamixel_sdk/group_sync_read.py @@ -19,7 +19,7 @@ # Author: Ryu Woon Jung (Leon) -from robotis_def import * +from .robotis_def import * class GroupSyncRead: def __init__(self, port, ph, start_address, data_length): @@ -53,7 +53,7 @@ def addParam(self, id): if id in self.data_dict: # id already exist return False - + self.data_dict[id] = [] # [0] * self.data_length self.is_param_changed = True @@ -65,11 +65,11 @@ def removeParam(self, id): if not id in self.data_dict: # NOT exist return - + del self.data_dict[id] self.is_param_changed = True - + def clearParam(self): if self.ph.getProtocolVersion() == 1.0: return @@ -79,7 +79,7 @@ def clearParam(self): 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 == True or not self.param: self.makeParam() @@ -109,7 +109,7 @@ def rxPacket(self): def txRxPacket(self): if self.ph.getProtocolVersion() == 1.0: return COMM_NOT_AVAILABLE - + result = COMM_TX_FAIL result = self.txPacket() @@ -126,7 +126,7 @@ def isAvailable(self, id, address, data_length): return False return True - + def getData(self, id, address, data_length): if self.isAvailable(id, address, data_length) == False: return 0 @@ -138,4 +138,4 @@ def getData(self, id, address, data_length): elif data_length == 4: return DXL_MAKEDWORD(DXL_MAKEWORD(self.data_dict[id][address - self.start_address + 0], self.data_dict[id][address - self.start_address + 1]), DXL_MAKEWORD(self.data_dict[id][address - self.start_address + 2], self.data_dict[id][address - self.start_address + 3])) else: - return 0 \ No newline at end of file + return 0 diff --git a/python/src/dynamixel_sdk/group_sync_write.py b/python/src/dynamixel_sdk/group_sync_write.py index 75b7b968..be6c2775 100644 --- a/python/src/dynamixel_sdk/group_sync_write.py +++ b/python/src/dynamixel_sdk/group_sync_write.py @@ -19,7 +19,7 @@ # Author: Ryu Woon Jung (Leon) -from robotis_def import * +from .robotis_def import * class GroupSyncWrite: def __init__(self, port, ph, start_address, data_length): @@ -50,7 +50,7 @@ def makeParam(self): def addParam(self, id, data): if (id in self.data_dict): # id already exist return False - + if len(data) > self.data_length: # input data is longer than set return False @@ -62,11 +62,11 @@ def addParam(self, id, data): def removeParam(self, id): if not id in self.data_dict: # NOT exist return - + del self.data_dict[id] self.is_param_changed = True - + def changeParam(self, id, data): if not id in self.data_dict: # NOT exist return False @@ -85,8 +85,8 @@ def clearParam(self): def txPacket(self): if len(self.data_dict.keys()) == 0: return COMM_NOT_AVAILABLE - + if self.is_param_changed == 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)) \ No newline at end of file + 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 index 84617d4f..f37f98a8 100644 --- a/python/src/dynamixel_sdk/packet_handler.py +++ b/python/src/dynamixel_sdk/packet_handler.py @@ -19,8 +19,10 @@ # Author: Ryu Woon Jung (Leon) -from protocol1_packet_handler import * -from protocol2_packet_handler import * +# from protocol1_packet_handler import * +# from protocol2_packet_handler import * +from .protocol1_packet_handler import * +from .protocol2_packet_handler import * def PacketHandler(protocol_version): if protocol_version == 1.0: @@ -28,4 +30,4 @@ def PacketHandler(protocol_version): elif protocol_version == 2.0: return Protocol2PacketHandler() else: - return Protocol2PacketHandler() \ No newline at end of file + return Protocol2PacketHandler() diff --git a/python/src/dynamixel_sdk/port_handler.py b/python/src/dynamixel_sdk/port_handler.py index d542ba90..a363be14 100644 --- a/python/src/dynamixel_sdk/port_handler.py +++ b/python/src/dynamixel_sdk/port_handler.py @@ -21,18 +21,50 @@ import platform +# if platform.system() == 'Linux': +# from port_handler_linux import * +# +# class PortHandler(PortHandlerLinux): +# pass +# elif platform.system() == 'Windows': +# from port_handler_windows import * +# +# class PortHandler(PortHandlerWindows): +# pass +# elif platform.system() == 'Darwin': +# from port_handler_mac import * +# +# class PortHandler(PortHandlerMac): +# pass + if platform.system() == 'Linux': - from port_handler_linux import * + from .port_handler_linux import * class PortHandler(PortHandlerLinux): pass elif platform.system() == 'Windows': - from port_handler_windows import * + from .port_handler_windows import * class PortHandler(PortHandlerWindows): pass elif platform.system() == 'Darwin': - from port_handler_mac import * + from .port_handler_mac import * class PortHandler(PortHandlerMac): pass + +# if platform.system() == 'Linux': +# from . import port_handler_linux +# +# class PortHandler(PortHandlerLinux): +# pass +# elif platform.system() == 'Windows': +# from . import port_handler_windows +# +# class PortHandler(port_handler_windows.PortHandlerWindows): +# pass +# elif platform.system() == 'Darwin': +# from . import port_handler_mac +# +# class PortHandler(PortHandlerMac): +# pass diff --git a/python/src/dynamixel_sdk/port_handler_linux.py b/python/src/dynamixel_sdk/port_handler_linux.py index 0adee5b3..317bcb2d 100644 --- a/python/src/dynamixel_sdk/port_handler_linux.py +++ b/python/src/dynamixel_sdk/port_handler_linux.py @@ -22,7 +22,7 @@ import time import serial -LATENCY_TIMER = 4 #16 +LATENCY_TIMER = 16 DEFAULT_BAUDRATE = 1000000 class PortHandlerLinux(object): @@ -38,7 +38,7 @@ def __init__(self, port_name): def openPort(self): return self.setBaudRate(self.baudrate) - + def closePort(self): self.ser.close() @@ -54,12 +54,7 @@ def getPortName(self): def setBaudRate(self, baudrate): baud = self.getCFlagBaud(baudrate) - # When the port is already open, it will be closed and reopened with the new setting. - # self.closePort() - if baud <= 0: - # self.setupPort(38400) - # self.baudrate = baudrate return False else: self.baudrate = baudrate @@ -72,21 +67,11 @@ def getBytesAvailable(self): return self.ser.in_waiting def readPort(self, length): - # read_bytes = self.ser.read(length) read_bytes = [] - read_bytes.extend([ord(ch) for ch in self.ser.read(length)]) - # for i in range(0, len(read_bytes)): - # read_bytes[i] = int.frombytes(read_bytes[i], byteorder = 'little') - - # for i in read_bytes: - # print i return read_bytes def writePort(self, packet): - # for i in packet: - # print i - return self.ser.write(packet) def setPacketTimeout(self, packet_length): @@ -101,13 +86,11 @@ def isPacketTimeout(self): if self.getTimeSinceStart() > self.packet_timeout: self.packet_timeout = 0 return True - + return False def getCurrentTime(self): - millis = round(time.time() * 1000000000) / 1000000.0 - # print "%.6f" % millis - return millis + return round(time.time() * 1000000000) / 1000000.0 def getTimeSinceStart(self): time = self.getCurrentTime() - self.packet_start_time @@ -122,8 +105,8 @@ def setupPort(self, cflag_baud): baudrate = self.baudrate, # parity = serial.PARITY_ODD, # stopbits = serial.STOPBITS_TWO, - bytesize = serial.EIGHTBITS #bytesize = serial.SEVENBITS - , timeout = 0 + bytesize = serial.EIGHTBITS, + timeout = 0 ) self.ser.reset_input_buffer() @@ -170,4 +153,4 @@ def getCFlagBaud(self, baudrate): elif baudrate == 4000000: return 4000000 else: - return -1 \ No newline at end of file + return -1 diff --git a/python/src/dynamixel_sdk/port_handler_windows.py b/python/src/dynamixel_sdk/port_handler_windows.py index 95a92d46..153f329a 100644 --- a/python/src/dynamixel_sdk/port_handler_windows.py +++ b/python/src/dynamixel_sdk/port_handler_windows.py @@ -22,12 +22,12 @@ import time import serial -LATENCY_TIMER = 16 +LATENCY_TIMER = 1 DEFAULT_BAUDRATE = 1000000 class PortHandlerWindows(object): def __init__(self, port_name): - self.socket_fd = 0.0 + self.is_open = False self.baudrate = DEFAULT_BAUDRATE self.packet_start_time = 0.0 self.packet_timeout = 0.0 @@ -38,9 +38,10 @@ def __init__(self, port_name): def openPort(self): return self.setBaudRate(self.baudrate) - + def closePort(self): self.ser.close() + self.is_open = False def clearPort(self): self.ser.flush() @@ -54,63 +55,29 @@ def getPortName(self): def setBaudRate(self, baudrate): baud = self.getCFlagBaud(baudrate) - self.closePort() - if baud <= 0: - setupPort(38400) - self.baudrate = baudrate - return setCustomBaudrate(baudrate) + return False else: self.baudrate = baudrate - return setupPort(baud) - - # TODO:more simplify + return self.setupPort(baud) def getBaudRate(self): return self.baudrate def getBytesAvailable(self): - pass + return self.ser.in_waiting - # TODO: fill + def readPort(self, length): + read_bytes = [] + read_bytes = self.ser.readline() + # read_bytes.extend([ord(ch) for ch in self.ser.read(length)]) + # print("A : %d" % len(read_bytes)) + return read_bytes - def readPort(self, packet, length): - pass - - # TODO: fill - def writePort(self, packet): - ser.write(packet) - - -# ser.isOpen() - -# print 'Enter your commands below.\r\nInsert "exit" to leave the application.' - -# input=1 -# while 1 : -# # get keyboard input -# input = raw_input(">> ") -# # Python 3 users -# # input = input(">> ") -# if input == 'exit': -# ser.close() -# exit() -# else: -# # send the character to the device -# # (note that I happend a \r\n carriage return and line feed to the characters - this is requested by my device) -# ser.write(input) -# # out = '' -# # # let's wait one second before reading output (let's give device time to answer) -# # time.sleep(1) -# # while ser.inWaiting() > 0: -# # out += ser.read(1) - -# # if out != '': -# # print ">>" + out - - - + res = self.ser.write(packet) + # print("B: %s" % packet) + return res def setPacketTimeout(self, packet_length): self.packet_start_time = self.getCurrentTime() @@ -121,16 +88,14 @@ def setPacketTimeout(self, msec): self.packet_timeout = msec def isPacketTimeout(self): - if self.getTimeSinceStart() > packet_timeout: + if self.getTimeSinceStart() > self.packet_timeout: self.packet_timeout = 0 return True - + return False def getCurrentTime(self): - pass - - #TODO : fill + return round(time.time() * 1000000000) / 1000000.0 def getTimeSinceStart(self): time = self.getCurrentTime() - self.packet_start_time @@ -140,62 +105,62 @@ def getTimeSinceStart(self): return time def setupPort(self, cflag_baud): - - # TODO: Error exception + if self.is_open: + self.closePort() self.ser = serial.Serial( - port = self.port_name, #'/dev/ttyUSB0', - baudrate = self.baudrate, # 1000000, - parity = serial.PARITY_ODD, - stopbits = serial.STOPBITS_TWO, - bytesize = serial.SEVENBITS + port = self.port_name, + baudrate = self.baudrate, + # parity = serial.PARITY_ODD, + # stopbits = serial.STOPBITS_TWO, + bytesize = serial.EIGHTBITS, + timeout = 0 ) - self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 + self.is_open = True - return True + self.ser.reset_input_buffer() - def setCustomBaudrate(self, speed): - pass + self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 - #TODO: fill + return True def getCFlagBaud(self, baudrate): - if baudrate is 9600: + if baudrate == 9600: return 9600 - elif baudrate is 19200: + elif baudrate == 19200: return 19200 - elif baudrate is 38400: + elif baudrate == 38400: return 38400 - elif baudrate is 57600: + elif baudrate == 57600: return 57600 - elif baudrate is 115200: + elif baudrate == 115200: return 115200 - elif baudrate is 230400: + elif baudrate == 230400: return 230400 - elif baudrate is 460800: + elif baudrate == 460800: return 460800 - elif baudrate is 500000: + elif baudrate == 500000: return 500000 - elif baudrate is 576000: + elif baudrate == 576000: return 576000 - elif baudrate is 921600: + elif baudrate == 921600: return 921600 - elif baudrate is 1000000: + elif baudrate == 1000000: return 1000000 - elif baudrate is 1152000: + elif baudrate == 1152000: return 1152000 - elif baudrate is 1500000: + elif baudrate == 1500000: return 1500000 - elif baudrate is 2000000: + elif baudrate == 2000000: return 2000000 - elif baudrate is 2500000: + elif baudrate == 2500000: return 2500000 - elif baudrate is 3000000: + elif baudrate == 3000000: return 3000000 - elif baudrate is 3500000: + elif baudrate == 3500000: return 3500000 - elif baudrate is 4000000: + elif baudrate == 4000000: return 4000000 else: - return -1 \ No newline at end of file + return -1 diff --git a/python/src/dynamixel_sdk/protocol1_packet_handler.py b/python/src/dynamixel_sdk/protocol1_packet_handler.py index 134f082f..8e1f0eca 100644 --- a/python/src/dynamixel_sdk/protocol1_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -19,7 +19,7 @@ # Author: Ryu Woon Jung (Leon) -from robotis_def import * +from .robotis_def import * TXPACKET_MAX_LEN = 250 RXPACKET_MAX_LEN = 250 @@ -146,7 +146,7 @@ def rxPacket(self, port, rxpacket=[]): 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 @@ -157,14 +157,12 @@ def rxPacket(self, port, rxpacket=[]): if port.isPacketTimeout() == True: if rx_length == 0: result = COMM_RX_TIMEOUT - print "COMM_RX_TIMEOUT 1" else: result = COMM_RX_CORRUPT - print "COMM_RX_CORRUPT 1" break else: continue - + # calculate checksum for i in range(2, wait_length - 1): # except header, checksum checksum += rxpacket[i] @@ -175,26 +173,20 @@ def rxPacket(self, port, rxpacket=[]): result = COMM_SUCCESS else: result = COMM_RX_CORRUPT - print "COMM_RX_CORRUPT 2" - - print("checksum / rxpacket : ", checksum, rxpacket[wait_length - 1]) - break else: #remove unnecessary packets del rxpacket[0 : idx] rx_length -= idx - + else: # check timeout if port.isPacketTimeout() == True: if rx_length == 0: result = COMM_RX_TIMEOUT - print "COMM_RX_TIMEOUT 3" else: result = COMM_RX_CORRUPT - print "COMM_RX_CORRUPT 3" break port.is_using = False @@ -225,7 +217,7 @@ def txRxPacket(self, port, txpacket, rxpacket=[], error=0): 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, rxpacket) @@ -238,7 +230,7 @@ def txRxPacket(self, port, txpacket, rxpacket=[], error=0): error = rxpacket[PKT_ERROR] return rxpacket, result, error - + def ping(self, port, id, model_number=0, error=0): result = COMM_TX_FAIL @@ -301,7 +293,7 @@ def readTx(self, port, id, address, length): txpacket[PKT_LENGTH] = 4 txpacket[PKT_INSTRUCTION] = INST_READ txpacket[PKT_PARAMETER0+0] = address - txpacket[PKT_PARAMETER0+1] = length + txpacket[PKT_PARAMETER0+1] = length result = self.txPacket(port, txpacket) @@ -315,7 +307,7 @@ def readRx(self, port, id, length, data=[], error=0): result = COMM_TX_FAIL rxpacket = [] - + if len(data) != 0: data = [] @@ -330,7 +322,7 @@ def readRx(self, port, id, length, data=[], error=0): error = rxpacket[PKT_ERROR] data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) - + del rxpacket[:]; del rxpacket return data, result, error @@ -355,7 +347,7 @@ def readTxRx(self, port, id, address, length, data=[], error=0): if result == COMM_SUCCESS: if error != 0: error = rxpacket[PKT_ERROR] - + data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) del rxpacket[:]; del rxpacket @@ -382,7 +374,7 @@ def read2ByteRx(self, port, id, data=[], error=0): data_read = [0] * 2 data_read, result, error = self.readRx(port, id, 2, data_read, error) if result == COMM_SUCCESS: - data = DXL_MAKEWORD(data_read[0], data_read[1]) + data = DXL_MAKEWORD(data_read[0], data_read[1]) return data, result, error def read2ByteTxRx(self, port, id, address, data=[], error=0): data_read = [0] * 2 @@ -403,7 +395,7 @@ def read4ByteTxRx(self, port, id, address, data=[], error=0): data_read = [0] * 4 data_read, result, error = self.readTxRx(port, id, address, 4, data_read, error) if result == COMM_SUCCESS: - data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) + data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) return data, result, error def writeTxOnly(self, port, id, address, length, data): @@ -416,7 +408,7 @@ def writeTxOnly(self, port, id, address, length, data): 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) @@ -425,12 +417,12 @@ def writeTxOnly(self, port, id, address, length, data): del txpacket[:]; del txpacket return result - def writeTxRx(self, port, id, address, length, data, error=0): + def writeTxRx(self, port, id, address, length, data, error=0): result = COMM_TX_FAIL txpacket = [0] * (length + 7) rxpacket = [] - + txpacket[PKT_ID] = id txpacket[PKT_LENGTH] = length + 3 txpacket[PKT_INSTRUCTION] = INST_WRITE @@ -473,7 +465,7 @@ def regWriteTxOnly(self, port, id, address, length, data): 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) @@ -506,7 +498,7 @@ def syncReadTx(self, port, start_address, data_length, param, param_length): def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): result = COMM_TX_FAIL - txpacket = [0] * (param_length + 8) + txpacket = [0] * (param_length + 8) # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM txpacket[PKT_ID] = BROADCAST_ID @@ -514,7 +506,7 @@ def syncWriteTxOnly(self, port, start_address, data_length, param, param_length) 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) @@ -548,4 +540,4 @@ def bulkReadTx(self, port, param, param_length): return result def bulkWriteTxOnly(self, port, param, param_length): - return COMM_NOT_AVAILABLE \ No newline at end of file + return COMM_NOT_AVAILABLE diff --git a/python/src/dynamixel_sdk/protocol2_packet_handler.py b/python/src/dynamixel_sdk/protocol2_packet_handler.py index 57bd0f52..12501706 100644 --- a/python/src/dynamixel_sdk/protocol2_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol2_packet_handler.py @@ -19,7 +19,7 @@ # Author: Ryu Woon Jung (Leon) -from robotis_def import * +from .robotis_def import * TXPACKET_MAX_LEN = 4 * 1024 RXPACKET_MAX_LEN = 4 * 1024 @@ -75,7 +75,7 @@ def getTxRxResult(self, result): def getRxPacketError(self, error): if error & ERRBIT_ALERT: - return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!" + return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!" not_alert_error = error & ~ERRBIT_ALERT @@ -103,7 +103,7 @@ def getRxPacketError(self, error): return "[RxPacketError] Writing or Reading is not available to target address!" else: - return "[RxPacketError] Unknown error code!" + return "[RxPacketError] Unknown error code!" def updateCRC(self, crc_accum, data_blk_ptr, data_blk_size): crc_table = [0x0000, @@ -169,7 +169,7 @@ def addStuffing(self, packet): 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 @@ -178,7 +178,7 @@ def addStuffing(self, packet): 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) @@ -194,10 +194,10 @@ def removeStuffing(self, packet): # 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] @@ -230,7 +230,7 @@ def txPacket(self, port, txpacket): # 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) @@ -240,7 +240,7 @@ def txPacket(self, port, txpacket): if total_packet_length != written_packet_length: port.is_using = False return COMM_TX_FAIL - + return COMM_SUCCESS def rxPacket(self, port, rxpacket=[]): @@ -257,14 +257,14 @@ def rxPacket(self, port, rxpacket=[]): 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 @@ -278,7 +278,7 @@ def rxPacket(self, port, rxpacket=[]): break else: continue - + crc = DXL_MAKEWORD(rxpacket[wait_length-2], rxpacket[wait_length-1]) if self.updateCRC(0, rxpacket, wait_length - 2) == crc: @@ -286,13 +286,13 @@ def rxPacket(self, port, rxpacket=[]): else: result = COMM_RX_CORRUPT break - + else: # remove unnecessary packets del rxpacket[0 : idx] - + rx_length = rx_length - idx - + else: if port.isPacketTimeout() == True: if rx_length == 0: @@ -300,7 +300,7 @@ def rxPacket(self, port, rxpacket=[]): else: result = COMM_RX_CORRUPT break - + port.is_using = False if result == COMM_SUCCESS: @@ -346,7 +346,7 @@ def txRxPacket(self, port, txpacket, rxpacket=[], error=0): error = rxpacket[PKT_ERROR] return rxpacket, result, error - + def ping(self, port, id, model_number=0, error=0): result = COMM_TX_FAIL @@ -425,7 +425,7 @@ def broadcastPing(self, port, data_list={}): if rx_length == 0: return data_list, result - + else: result = COMM_RX_CORRUPT @@ -437,7 +437,7 @@ def broadcastPing(self, port, data_list={}): # remove unnecessary packets del rxpacket[0 : idx] rx_length = rx_length - idx - + return data_list, result def action(self, port, id): @@ -492,7 +492,7 @@ def readTx(self, port, id, address, length): 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) + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(length) result = self.txPacket(port, txpacket) @@ -506,7 +506,7 @@ def readRx(self, port, id, length, data=[], error=0): result = COMM_TX_FAIL rxpacket = [] - + if len(data) != 0: data = [] @@ -521,7 +521,7 @@ def readRx(self, port, id, length, data=[], error=0): error = rxpacket[PKT_ERROR] data.extend(rxpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length]) - + del rxpacket[:]; del rxpacket return data, result, error @@ -544,17 +544,17 @@ def readTxRx(self, port, id, address, length, data=[], error=0): 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) + txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(length) rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket, error) if result == COMM_SUCCESS: if error != 0: error = rxpacket[PKT_ERROR] - + data.extend(rxpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length]) del rxpacket[:]; del rxpacket - return data, result, error + return data, result, error def read1ByteTx(self, port, id, address): return self.readTx(port, id, address, 1) @@ -577,7 +577,7 @@ def read2ByteRx(self, port, id, data=[], error=0): data_read = [0] * 2 data_read, result, error = self.readRx(port, id, 2, data_read, error) if result == COMM_SUCCESS: - data = DXL_MAKEWORD(data_read[0], data_read[1]) + data = DXL_MAKEWORD(data_read[0], data_read[1]) return data, result, error def read2ByteTxRx(self, port, id, address, data=[], error=0): data_read = [0] * 2 @@ -598,7 +598,7 @@ def read4ByteTxRx(self, port, id, address, data=[], error=0): data_read = [0] * 4 data_read, result, error = self.readTxRx(port, id, address, 4, data_read, error) if result == COMM_SUCCESS: - data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) + data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) return data, result, error @@ -614,7 +614,7 @@ def writeTxOnly(self, port, id, address, length, data): 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) @@ -623,12 +623,12 @@ def writeTxOnly(self, port, id, address, length, data): del txpacket[:]; del txpacket return result - def writeTxRx(self, port, id, address, length, data, error=0): + def writeTxRx(self, port, id, address, length, data, error=0): result = COMM_TX_FAIL txpacket = [0] * (length + 12) rxpacket = [] - + txpacket[PKT_ID] = id txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5) txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5) @@ -674,7 +674,7 @@ def regWriteTxOnly(self, port, id, address, length, data): 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) @@ -694,7 +694,7 @@ def regWriteTxRx(self, port, id, address, length, data, error=0): 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+1] = DXL_HIBYTE(address) txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + length] = data[0 : length] @@ -706,9 +706,9 @@ def regWriteTxRx(self, port, id, address, length, data, error=0): def syncReadTx(self, port, start_address, data_length, param, param_length): result = COMM_TX_FAIL - txpacket = [0] * (param_length + 14) + 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 @@ -730,7 +730,7 @@ def syncReadTx(self, port, start_address, data_length, param, param_length): def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): result = COMM_TX_FAIL - txpacket = [0] * (param_length + 14) + 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 @@ -741,7 +741,7 @@ def syncWriteTxOnly(self, port, start_address, data_length, param, param_length) 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) @@ -790,4 +790,4 @@ def bulkWriteTxOnly(self, port, param, param_length): result = self.txRxPacket(port, txpacket) del txpacket[:]; del txpacket - return result \ No newline at end of file + return result diff --git a/python/tests/protocol1_0/ping.py b/python/tests/protocol1_0/ping.py index 37c648d9..14ef12b0 100644 --- a/python/tests/protocol1_0/ping.py +++ b/python/tests/protocol1_0/ping.py @@ -54,7 +54,7 @@ def getch(): # 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 +DEVICENAME = 'COM16' # Check which port is being used on your controller # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" # Initialize PortHandler instance @@ -69,20 +69,20 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + print("Failed to change the baudrate") + print("Press any key to terminate...") getch() quit() @@ -90,11 +90,11 @@ def getch(): # Get Dynamixel model number dxl_model_number, dxl_comm_result, dxl_error = packetHandler.ping(portHandler, DXL_ID) if dxl_comm_result != COMM_SUCCESS: - print packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number) + print("[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number)) # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() diff --git a/python/tests/protocol1_0/read_write.py b/python/tests/protocol1_0/read_write.py index d4d7c448..bfc42ef4 100644 --- a/python/tests/protocol1_0/read_write.py +++ b/python/tests/protocol1_0/read_write.py @@ -59,7 +59,7 @@ def getch(): # 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 +DEVICENAME = 'COM16' # 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 @@ -84,53 +84,53 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel has been successfully connected" + print("Dynamixel has been successfully connected") while 1: - print "Press any key to continue! (or press ESC to quit!)" + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) - print "[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position) + 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 @@ -139,15 +139,15 @@ def getch(): if index == 0: index = 1 else: - index = 0 + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() diff --git a/python/tests/protocol2_0/ping.py b/python/tests/protocol2_0/ping.py index 2a630409..1d0869f2 100644 --- a/python/tests/protocol2_0/ping.py +++ b/python/tests/protocol2_0/ping.py @@ -47,15 +47,15 @@ def getch(): termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch -from ..src.dynamixel_sdk import * # Uses Dynamixel SDK library +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 -DXL_ID = 1 # Dynamixel ID : 1 +DXL_ID = 2 # Dynamixel ID : 1 BAUDRATE = 57600 # Dynamixel default baudrate : 57600 -DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller +DEVICENAME = 'COM16' # Check which port is being used on your controller # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" # Initialize PortHandler instance @@ -70,20 +70,20 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + print("Failed to change the baudrate") + print("Press any key to terminate...") getch() quit() @@ -91,11 +91,11 @@ def getch(): # Get Dynamixel model number dxl_model_number, dxl_comm_result, dxl_error = packetHandler.ping(portHandler, DXL_ID) if dxl_comm_result != COMM_SUCCESS: - print packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number) + print("[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number)) # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() diff --git a/python/tests/protocol2_0/read_write.py b/python/tests/protocol2_0/read_write.py index e1b15513..d7432ccf 100644 --- a/python/tests/protocol2_0/read_write.py +++ b/python/tests/protocol2_0/read_write.py @@ -140,7 +140,7 @@ def getch(): if index == 0: index = 1 else: - index = 0 + index = 0 # Disable Dynamixel Torque @@ -151,4 +151,4 @@ def getch(): print packetHandler.getRxPacketError(dxl_error) # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() From 318cb33fbdf3e8a6d11bb0cce73aab1ffee808cf Mon Sep 17 00:00:00 2001 From: Leon Jung Date: Thu, 26 Apr 2018 21:22:27 +0900 Subject: [PATCH 08/25] added: Windows porting & Python 3 --- python/src/dynamixel_sdk/__init__.py | 6 - python/src/dynamixel_sdk/packet_handler.py | 2 - python/src/dynamixel_sdk/port_handler.py | 32 ----- .../src/dynamixel_sdk/port_handler_linux.py | 8 +- python/src/dynamixel_sdk/port_handler_mac.py | 129 ++++++------------ .../src/dynamixel_sdk/port_handler_windows.py | 5 +- python/tests/protocol1_0/bulk_read.py | 52 +++---- python/tests/protocol1_0/factory_reset.py | 56 ++++---- python/tests/protocol1_0/multi_port.py | 56 ++++---- python/tests/protocol1_0/ping.py | 2 +- python/tests/protocol1_0/read_write.py | 2 +- python/tests/protocol1_0/sync_write.py | 50 +++---- python/tests/protocol2_0/broadcast_ping.py | 16 +-- python/tests/protocol2_0/bulk_read_write.py | 48 +++---- python/tests/protocol2_0/factory_reset.py | 56 ++++---- python/tests/protocol2_0/indirect_address.py | 78 +++++------ python/tests/protocol2_0/multi_port.py | 64 ++++----- python/tests/protocol2_0/ping.py | 2 +- python/tests/protocol2_0/read_write.py | 34 ++--- python/tests/protocol2_0/reboot.py | 22 +-- python/tests/protocol2_0/sync_read_write.py | 52 +++---- python/tests/protocol_combined.py | 55 ++++---- 22 files changed, 376 insertions(+), 451 deletions(-) diff --git a/python/src/dynamixel_sdk/__init__.py b/python/src/dynamixel_sdk/__init__.py index 3005ec31..bc227b66 100644 --- a/python/src/dynamixel_sdk/__init__.py +++ b/python/src/dynamixel_sdk/__init__.py @@ -19,12 +19,6 @@ # 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 * from .port_handler import * from .packet_handler import * from .group_sync_read import * diff --git a/python/src/dynamixel_sdk/packet_handler.py b/python/src/dynamixel_sdk/packet_handler.py index f37f98a8..f539a369 100644 --- a/python/src/dynamixel_sdk/packet_handler.py +++ b/python/src/dynamixel_sdk/packet_handler.py @@ -19,8 +19,6 @@ # Author: Ryu Woon Jung (Leon) -# from protocol1_packet_handler import * -# from protocol2_packet_handler import * from .protocol1_packet_handler import * from .protocol2_packet_handler import * diff --git a/python/src/dynamixel_sdk/port_handler.py b/python/src/dynamixel_sdk/port_handler.py index a363be14..3e0de0ea 100644 --- a/python/src/dynamixel_sdk/port_handler.py +++ b/python/src/dynamixel_sdk/port_handler.py @@ -21,22 +21,6 @@ import platform -# if platform.system() == 'Linux': -# from port_handler_linux import * -# -# class PortHandler(PortHandlerLinux): -# pass -# elif platform.system() == 'Windows': -# from port_handler_windows import * -# -# class PortHandler(PortHandlerWindows): -# pass -# elif platform.system() == 'Darwin': -# from port_handler_mac import * -# -# class PortHandler(PortHandlerMac): -# pass - if platform.system() == 'Linux': from .port_handler_linux import * @@ -52,19 +36,3 @@ class PortHandler(PortHandlerWindows): class PortHandler(PortHandlerMac): pass - -# if platform.system() == 'Linux': -# from . import port_handler_linux -# -# class PortHandler(PortHandlerLinux): -# pass -# elif platform.system() == 'Windows': -# from . import port_handler_windows -# -# class PortHandler(port_handler_windows.PortHandlerWindows): -# pass -# elif platform.system() == 'Darwin': -# from . import port_handler_mac -# -# class PortHandler(PortHandlerMac): -# pass diff --git a/python/src/dynamixel_sdk/port_handler_linux.py b/python/src/dynamixel_sdk/port_handler_linux.py index 317bcb2d..79e3cbd6 100644 --- a/python/src/dynamixel_sdk/port_handler_linux.py +++ b/python/src/dynamixel_sdk/port_handler_linux.py @@ -27,7 +27,7 @@ class PortHandlerLinux(object): def __init__(self, port_name): - self.socket_fd = 0.0 + self.is_open = False self.baudrate = DEFAULT_BAUDRATE self.packet_start_time = 0.0 self.packet_timeout = 0.0 @@ -41,6 +41,7 @@ def openPort(self): def closePort(self): self.ser.close() + self.is_open = False def clearPort(self): self.ser.flush() @@ -100,6 +101,9 @@ def getTimeSinceStart(self): return time def setupPort(self, cflag_baud): + if self.is_open: + self.closePort() + self.ser = serial.Serial( port = self.port_name, baudrate = self.baudrate, @@ -109,6 +113,8 @@ def setupPort(self, cflag_baud): timeout = 0 ) + self.is_open = True + self.ser.reset_input_buffer() self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 diff --git a/python/src/dynamixel_sdk/port_handler_mac.py b/python/src/dynamixel_sdk/port_handler_mac.py index 4ae0c6a8..90396303 100644 --- a/python/src/dynamixel_sdk/port_handler_mac.py +++ b/python/src/dynamixel_sdk/port_handler_mac.py @@ -27,7 +27,7 @@ class PortHandlerMac(object): def __init__(self, port_name): - self.socket_fd = 0.0 + self.is_open = False self.baudrate = DEFAULT_BAUDRATE self.packet_start_time = 0.0 self.packet_timeout = 0.0 @@ -38,9 +38,10 @@ def __init__(self, port_name): def openPort(self): return self.setBaudRate(self.baudrate) - + def closePort(self): self.ser.close() + self.is_open = False def clearPort(self): self.ser.flush() @@ -54,63 +55,25 @@ def getPortName(self): def setBaudRate(self, baudrate): baud = self.getCFlagBaud(baudrate) - self.closePort() - if baud <= 0: - setupPort(38400) - self.baudrate = baudrate - return setCustomBaudrate(baudrate) + return False else: self.baudrate = baudrate - return setupPort(baud) - - # TODO:more simplify + return self.setupPort(baud) def getBaudRate(self): return self.baudrate def getBytesAvailable(self): - pass + return self.ser.in_waiting - # TODO: fill + def readPort(self, length): + read_bytes = [] + read_bytes.extend([ord(ch) for ch in self.ser.read(length)]) + return read_bytes - def readPort(self, packet, length): - pass - - # TODO: fill - def writePort(self, packet): - ser.write(packet) - - -# ser.isOpen() - -# print 'Enter your commands below.\r\nInsert "exit" to leave the application.' - -# input=1 -# while 1 : -# # get keyboard input -# input = raw_input(">> ") -# # Python 3 users -# # input = input(">> ") -# if input == 'exit': -# ser.close() -# exit() -# else: -# # send the character to the device -# # (note that I happend a \r\n carriage return and line feed to the characters - this is requested by my device) -# ser.write(input) -# # out = '' -# # # let's wait one second before reading output (let's give device time to answer) -# # time.sleep(1) -# # while ser.inWaiting() > 0: -# # out += ser.read(1) - -# # if out != '': -# # print ">>" + out - - - + return self.ser.write(packet) def setPacketTimeout(self, packet_length): self.packet_start_time = self.getCurrentTime() @@ -121,16 +84,14 @@ def setPacketTimeout(self, msec): self.packet_timeout = msec def isPacketTimeout(self): - if self.getTimeSinceStart() > packet_timeout: + if self.getTimeSinceStart() > self.packet_timeout: self.packet_timeout = 0 return True - + return False def getCurrentTime(self): - pass - - #TODO : fill + return round(time.time() * 1000000000) / 1000000.0 def getTimeSinceStart(self): time = self.getCurrentTime() - self.packet_start_time @@ -140,62 +101,62 @@ def getTimeSinceStart(self): return time def setupPort(self, cflag_baud): - - # TODO: Error exception + if self.is_open: + self.closePort() self.ser = serial.Serial( - port = self.port_name, #'/dev/ttyUSB0', - baudrate = self.baudrate, # 1000000, - parity = serial.PARITY_ODD, - stopbits = serial.STOPBITS_TWO, - bytesize = serial.SEVENBITS + port = self.port_name, + baudrate = self.baudrate, + # parity = serial.PARITY_ODD, + # stopbits = serial.STOPBITS_TWO, + bytesize = serial.EIGHTBITS, + timeout = 0 ) - self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 + self.is_open = True - return True + self.ser.reset_input_buffer() - def setCustomBaudrate(self, speed): - pass + self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 - #TODO: fill + return True def getCFlagBaud(self, baudrate): - if baudrate is 9600: + if baudrate == 9600: return 9600 - elif baudrate is 19200: + elif baudrate == 19200: return 19200 - elif baudrate is 38400: + elif baudrate == 38400: return 38400 - elif baudrate is 57600: + elif baudrate == 57600: return 57600 - elif baudrate is 115200: + elif baudrate == 115200: return 115200 - elif baudrate is 230400: + elif baudrate == 230400: return 230400 - elif baudrate is 460800: + elif baudrate == 460800: return 460800 - elif baudrate is 500000: + elif baudrate == 500000: return 500000 - elif baudrate is 576000: + elif baudrate == 576000: return 576000 - elif baudrate is 921600: + elif baudrate == 921600: return 921600 - elif baudrate is 1000000: + elif baudrate == 1000000: return 1000000 - elif baudrate is 1152000: + elif baudrate == 1152000: return 1152000 - elif baudrate is 1500000: + elif baudrate == 1500000: return 1500000 - elif baudrate is 2000000: + elif baudrate == 2000000: return 2000000 - elif baudrate is 2500000: + elif baudrate == 2500000: return 2500000 - elif baudrate is 3000000: + elif baudrate == 3000000: return 3000000 - elif baudrate is 3500000: + elif baudrate == 3500000: return 3500000 - elif baudrate is 4000000: + elif baudrate == 4000000: return 4000000 else: - return -1 \ No newline at end of file + return -1 diff --git a/python/src/dynamixel_sdk/port_handler_windows.py b/python/src/dynamixel_sdk/port_handler_windows.py index 153f329a..309b1061 100644 --- a/python/src/dynamixel_sdk/port_handler_windows.py +++ b/python/src/dynamixel_sdk/port_handler_windows.py @@ -68,15 +68,12 @@ def getBytesAvailable(self): return self.ser.in_waiting def readPort(self, length): - read_bytes = [] + # read_bytes = [] read_bytes = self.ser.readline() - # read_bytes.extend([ord(ch) for ch in self.ser.read(length)]) - # print("A : %d" % len(read_bytes)) return read_bytes def writePort(self, packet): res = self.ser.write(packet) - # print("B: %s" % packet) return res def setPacketTimeout(self, packet_length): diff --git a/python/tests/protocol1_0/bulk_read.py b/python/tests/protocol1_0/bulk_read.py index d32cf025..ffdc2afc 100644 --- a/python/tests/protocol1_0/bulk_read.py +++ b/python/tests/protocol1_0/bulk_read.py @@ -93,20 +93,20 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + print("Failed to change the baudrate") + print("Press any key to terminate...") getch() quit() @@ -114,51 +114,51 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL1_ID + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL2_ID + 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 + 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 + print("[ID:%03d] groupBulkRead addparam failed" % DXL2_ID) quit() while 1: - print "Press any key to continue! (or press ESC to quit!)" + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) while 1: # Bulkread present position and moving status @@ -169,13 +169,13 @@ def getch(): # 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 + 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 + print("[ID:%03d] groupBulkRead getdata failed" % DXL2_ID) quit() # Get Dynamixel#1 present position value @@ -184,7 +184,7 @@ def getch(): # 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) + 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 @@ -201,16 +201,16 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol1_0/factory_reset.py b/python/tests/protocol1_0/factory_reset.py index 4eb5852e..fb797f04 100644 --- a/python/tests/protocol1_0/factory_reset.py +++ b/python/tests/protocol1_0/factory_reset.py @@ -82,75 +82,75 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + 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() +print("Now the controller baudrate is : %d" % portHandler.getBaudRate()) # Try factoryreset -print "[ID:%03d] Try factoryreset : " % DXL_ID +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 packetHandler.getTxRxResult(dxl_comm_result) + print("Aborted") + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) quit() elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Wait for reset -print "Wait for reset..." +print("Wait for reset...") sleep(2.0) -print "[ID:%03d] factoryReset Success!" % DXL_ID +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 + 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..." + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "[ID:%03d] DXL baudnum is now : %d" % (DXL_ID, dxl_baudnum_read) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM) + 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 + print("Succeeded to change the controller baudrate to : %d" % BAUDRATE) else: - print "Failed to change the controller baudrate" - print "Press any key to terminate..." + print("Failed to change the controller baudrate") + print("Press any key to terminate...") quit() sleep(0.2) @@ -158,11 +158,11 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "[ID:%03d] Dynamixel Baudnum is now : %d" % (DXL_ID, dxl_baudnum_read) + 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 index 266e12f8..e89c795b 100644 --- a/python/tests/protocol1_0/multi_port.py +++ b/python/tests/protocol1_0/multi_port.py @@ -86,76 +86,76 @@ def getch(): # Open port1 if portHandler1.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL1_ID + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL2_ID + print("Dynamixel#%d has been successfully connected" % DXL2_ID) while 1: - print "Press any key to continue! (or press ESC to quit!)" + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) while 1: @@ -173,7 +173,7 @@ def getch(): 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) + 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 @@ -188,16 +188,16 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port1 portHandler1.closePort() diff --git a/python/tests/protocol1_0/ping.py b/python/tests/protocol1_0/ping.py index 14ef12b0..12d08c18 100644 --- a/python/tests/protocol1_0/ping.py +++ b/python/tests/protocol1_0/ping.py @@ -54,7 +54,7 @@ def getch(): # Default setting DXL_ID = 1 # Dynamixel ID : 1 BAUDRATE = 57600 # Dynamixel default baudrate : 57600 -DEVICENAME = 'COM16' # Check which port is being used on your controller +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 diff --git a/python/tests/protocol1_0/read_write.py b/python/tests/protocol1_0/read_write.py index bfc42ef4..2d8fa88e 100644 --- a/python/tests/protocol1_0/read_write.py +++ b/python/tests/protocol1_0/read_write.py @@ -59,7 +59,7 @@ def getch(): # Default setting DXL_ID = 1 # Dynamixel ID : 1 BAUDRATE = 57600 # Dynamixel default baudrate : 57600 -DEVICENAME = 'COM16' # Check which port is being used on your controller +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 diff --git a/python/tests/protocol1_0/sync_write.py b/python/tests/protocol1_0/sync_write.py index 632f2660..5f390b24 100644 --- a/python/tests/protocol1_0/sync_write.py +++ b/python/tests/protocol1_0/sync_write.py @@ -92,20 +92,20 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + print("Failed to change the baudrate") + print("Press any key to terminate...") getch() quit() @@ -113,23 +113,23 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL1_ID + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL2_ID + print("Dynamixel#%d has been successfully connected" % DXL2_ID) while 1: - print "Press any key to continue! (or press ESC to quit!)" + print("Press any key to continue! (or press ESC to quit!)") if getch() == chr(0x1b): break @@ -139,19 +139,19 @@ def getch(): # 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 + 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 + print("[ID:%03d] groupSyncWrite addparam failed" % DXL2_ID) quit() # Syncwrite goal position dxl_comm_result = groupSyncWrite.txPacket() if dxl_comm_result != COMM_SUCCESS: - print packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) # Clear syncwrite parameter storage groupSyncWrite.clearParam() @@ -160,18 +160,18 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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) + 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 @@ -186,16 +186,16 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol2_0/broadcast_ping.py b/python/tests/protocol2_0/broadcast_ping.py index bbd0ee07..ac58a9fc 100644 --- a/python/tests/protocol2_0/broadcast_ping.py +++ b/python/tests/protocol2_0/broadcast_ping.py @@ -68,31 +68,31 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + 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 packetHandler.getTxRxResult(dxl_comm_result) + 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]) + 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() \ No newline at end of file diff --git a/python/tests/protocol2_0/bulk_read_write.py b/python/tests/protocol2_0/bulk_read_write.py index fec70803..ca126484 100644 --- a/python/tests/protocol2_0/bulk_read_write.py +++ b/python/tests/protocol2_0/bulk_read_write.py @@ -97,20 +97,20 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + print("Failed to change the baudrate") + print("Press any key to terminate...") getch() quit() @@ -118,35 +118,35 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL1_ID + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL2_ID + 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 + 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 + print("[ID:%03d] groupBulkRead addparam failed" % DXL2_ID) quit() while 1: - print "Press any key to continue! (or press ESC to quit!)" + print("Press any key to continue! (or press ESC to quit!)") if getch() == chr(0x1b): break @@ -156,13 +156,13 @@ def getch(): # 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 + 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 + print("[ID:%03d] groupBulkWrite addparam failed" % DXL2_ID) quit() # Bulkwrite goal position and LED value @@ -182,13 +182,13 @@ def getch(): # 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 + 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 + print("[ID:%03d] groupBulkRead getdata failed" % DXL2_ID) quit() # Get present position value @@ -197,7 +197,7 @@ def getch(): # 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) + 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 @@ -214,16 +214,16 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol2_0/factory_reset.py b/python/tests/protocol2_0/factory_reset.py index edff990a..2ca496fe 100644 --- a/python/tests/protocol2_0/factory_reset.py +++ b/python/tests/protocol2_0/factory_reset.py @@ -84,75 +84,75 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + 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() +print("Now the controller baudrate is : %d" % portHandler.getBaudRate()) # Try factoryreset -print "[ID:%03d] Try factoryreset : " % DXL_ID +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 packetHandler.getTxRxResult(dxl_comm_result) + print("Aborted") + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) quit() elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Wait for reset -print "Wait for reset..." +print("Wait for reset...") sleep(2.0) -print "[ID:%03d] factoryReset Success!" % DXL_ID +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 + 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..." + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "[ID:%03d] DXL baudnum is now : %d" % (DXL_ID, dxl_baudnum_read) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM) + 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 + print("Succeeded to change the controller baudrate to : %d" % BAUDRATE) else: - print "Failed to change the controller baudrate" - print "Press any key to terminate..." + print("Failed to change the controller baudrate") + print("Press any key to terminate...") quit() sleep(0.2) @@ -160,11 +160,11 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "[ID:%03d] Dynamixel Baudnum is now : %d" % (DXL_ID, dxl_baudnum_read) + 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 index 34448c7e..3e259fb4 100644 --- a/python/tests/protocol2_0/indirect_address.py +++ b/python/tests/protocol2_0/indirect_address.py @@ -107,20 +107,20 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + print("Failed to change the baudrate") + print("Press any key to terminate...") getch() quit() @@ -129,81 +129,81 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "[ID:%03d] Dynamixel has been successfully connected" %DXL_ID + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 + print("[ID:%03d] groupSyncRead addparam failed" % DXL_ID) quit() while 1: - print "Press any key to continue! (or press ESC to quit!)" + print("Press any key to continue! (or press ESC to quit!)") if getch() == chr(0x1b): break @@ -214,7 +214,7 @@ def getch(): # 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 + print("[ID:%03d]groupSyncWrite addparam failed" % DXL_ID) quit() # Syncwrite all @@ -234,13 +234,13 @@ def getch(): # 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 + 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 + print("[ID:%03d] groupSyncRead getdata failed" % DXL_ID) quit() # Get Dynamixel present position value @@ -249,7 +249,7 @@ def getch(): # 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) + 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 @@ -266,9 +266,9 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol2_0/multi_port.py b/python/tests/protocol2_0/multi_port.py index 65cc28e1..3a4346bd 100644 --- a/python/tests/protocol2_0/multi_port.py +++ b/python/tests/protocol2_0/multi_port.py @@ -86,94 +86,94 @@ def getch(): # Open port1 if portHandler1.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL1_ID + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL2_ID + print("Dynamixel#%d has been successfully connected" % DXL2_ID) while 1: - print "Press any key to continue! (or press ESC to quit!)" + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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) + 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 @@ -188,16 +188,16 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port1 portHandler1.closePort() diff --git a/python/tests/protocol2_0/ping.py b/python/tests/protocol2_0/ping.py index 1d0869f2..462ccedf 100644 --- a/python/tests/protocol2_0/ping.py +++ b/python/tests/protocol2_0/ping.py @@ -55,7 +55,7 @@ def getch(): # Default setting DXL_ID = 2 # Dynamixel ID : 1 BAUDRATE = 57600 # Dynamixel default baudrate : 57600 -DEVICENAME = 'COM16' # Check which port is being used on your controller +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 diff --git a/python/tests/protocol2_0/read_write.py b/python/tests/protocol2_0/read_write.py index d7432ccf..355bbc1d 100644 --- a/python/tests/protocol2_0/read_write.py +++ b/python/tests/protocol2_0/read_write.py @@ -85,53 +85,53 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel has been successfully connected" + print("Dynamixel has been successfully connected") while 1: - print "Press any key to continue! (or press ESC to quit!)" + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) - print "[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position) + 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 @@ -146,9 +146,9 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port portHandler.closePort() diff --git a/python/tests/protocol2_0/reboot.py b/python/tests/protocol2_0/reboot.py index 8af82513..724d3f0a 100644 --- a/python/tests/protocol2_0/reboot.py +++ b/python/tests/protocol2_0/reboot.py @@ -69,37 +69,37 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + print("Failed to change the baudrate") + print("Press any key to terminate...") getch() quit() # Trigger -print "Press any key to reboot" +print("Press any key to reboot") getch() -print "See the Dynamixel LED flickering" +print("See the Dynamixel LED flickering") # Try reboot # Dynamixel LED will flicker while it reboots dxl_comm_result, dxl_error = packetHandler.reboot(portHandler, DXL_ID) if dxl_comm_result != COMM_SUCCESS: - print(packetHandler.getTxRxResult(dxl_comm_result)) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print(packetHandler.getRxPacketError(dxl_error)) + print("%s" % packetHandler.getRxPacketError(dxl_error)) -print "[ID:%03d] reboot Succeeded\n" % DXL_ID +print("[ID:%03d] reboot Succeeded\n" % DXL_ID) # Close port diff --git a/python/tests/protocol2_0/sync_read_write.py b/python/tests/protocol2_0/sync_read_write.py index 7be61d29..3acab67c 100644 --- a/python/tests/protocol2_0/sync_read_write.py +++ b/python/tests/protocol2_0/sync_read_write.py @@ -95,20 +95,20 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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" + print("Succeeded to change the baudrate") else: - print "Failed to change the baudrate" - print "Press any key to terminate..." + print("Failed to change the baudrate") + print("Press any key to terminate...") getch() quit() @@ -116,35 +116,35 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL1_ID + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL2_ID + 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 + 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 + print("[ID:%03d] groupSyncRead addparam failed" % DXL2_ID) quit() while 1: - print "Press any key to continue! (or press ESC to quit!)" + print("Press any key to continue! (or press ESC to quit!)") if getch() == chr(0x1b): break @@ -154,19 +154,19 @@ def getch(): # 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 + 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 + print("[ID:%03d] groupSyncWrite addparam failed" % DXL2_ID) quit() # Syncwrite goal position dxl_comm_result = groupSyncWrite.txPacket() if dxl_comm_result != COMM_SUCCESS: - print packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) # Clear syncwrite parameter storage groupSyncWrite.clearParam() @@ -175,18 +175,18 @@ def getch(): # Syncread present position dxl_comm_result = groupSyncRead.txRxPacket() if dxl_comm_result != COMM_SUCCESS: - print packetHandler.getTxRxResult(dxl_comm_result) + 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 + 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 + print("[ID:%03d] groupSyncRead getdata failed" % DXL2_ID) quit() # Get Dynamixel#1 present position value @@ -195,7 +195,7 @@ def getch(): # 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) + 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 @@ -212,16 +212,16 @@ def getch(): # 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler.getRxPacketError(dxl_error) + print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol_combined.py b/python/tests/protocol_combined.py index b3c2e4b2..8e241519 100644 --- a/python/tests/protocol_combined.py +++ b/python/tests/protocol_combined.py @@ -100,76 +100,77 @@ def getch(): # Open port if portHandler.openPort(): - print "Succeeded to open the port" + print("Succeeded to open the port") else: - print "Failed to open the port" - print "Press any key to terminate..." + print("Failed to open the port") + print("Press any key to terminate...") getch() quit() -# Open port + +# Set port baudrate if portHandler.setBaudRate(BAUDRATE): - print "Succeeded to open the port" + print("Succeeded to change the baudrate") else: - print "Failed to open the port" - print "Press any key to terminate..." + 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 packetHandler1.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler1.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler1.getRxPacketError(dxl_error) + print("%s" % packetHandler1.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL1_ID + 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 packetHandler2.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler2.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler2.getRxPacketError(dxl_error) + print("%s" % packetHandler2.getRxPacketError(dxl_error)) else: - print "Dynamixel#%d has been successfully connected" % DXL2_ID + print("Dynamixel#%d has been successfully connected" % DXL2_ID) while 1: - print "Press any key to continue! (or press ESC to quit!)" + 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 packetHandler1.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler1.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler2.getRxPacketError(dxl_error) + 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 packetHandler1.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler2.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler2.getRxPacketError(dxl_error) + 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 packetHandler1.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler1.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler2.getRxPacketError(dxl_error) + 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 packetHandler1.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler2.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler2.getRxPacketError(dxl_error) + 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) + 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 @@ -184,16 +185,16 @@ def getch(): # 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 packetHandler1.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler1.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler2.getRxPacketError(dxl_error) + 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 packetHandler1.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler2.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: - print packetHandler2.getRxPacketError(dxl_error) + print("%s" % packetHandler2.getRxPacketError(dxl_error)) # Close port portHandler.closePort() From 4d31ff39b9889f2175f8e98be08885f1a5a9ffe5 Mon Sep 17 00:00:00 2001 From: leon Date: Thu, 26 Apr 2018 12:42:06 +0900 Subject: [PATCH 09/25] added: Windows porting & Python 3 --- python/tests/protocol1_0/bulk_read.py | 8 ++++---- python/tests/protocol1_0/sync_write.py | 4 ++-- python/tests/protocol2_0/broadcast_ping.py | 4 ++-- python/tests/protocol2_0/bulk_read_write.py | 10 +++++----- python/tests/protocol2_0/indirect_address.py | 10 +++++----- python/tests/protocol2_0/reboot.py | 2 +- python/tests/protocol2_0/sync_read_write.py | 4 ++-- 7 files changed, 21 insertions(+), 21 deletions(-) diff --git a/python/tests/protocol1_0/bulk_read.py b/python/tests/protocol1_0/bulk_read.py index ffdc2afc..9562ea12 100644 --- a/python/tests/protocol1_0/bulk_read.py +++ b/python/tests/protocol1_0/bulk_read.py @@ -159,12 +159,12 @@ def getch(): 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 packetHandler.getTxRxResult(dxl_comm_result) + 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) @@ -193,7 +193,7 @@ def getch(): if index == 0: index = 1 else: - index = 0 + index = 0 # Clear bulkread parameter storage groupBulkRead.clearParam() @@ -213,4 +213,4 @@ def getch(): print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() diff --git a/python/tests/protocol1_0/sync_write.py b/python/tests/protocol1_0/sync_write.py index 5f390b24..46a571c8 100644 --- a/python/tests/protocol1_0/sync_write.py +++ b/python/tests/protocol1_0/sync_write.py @@ -180,7 +180,7 @@ def getch(): if index == 0: index = 1 else: - index = 0 + index = 0 # Disable Dynamixel#1 Torque @@ -198,4 +198,4 @@ def getch(): print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() diff --git a/python/tests/protocol2_0/broadcast_ping.py b/python/tests/protocol2_0/broadcast_ping.py index ac58a9fc..6e02c527 100644 --- a/python/tests/protocol2_0/broadcast_ping.py +++ b/python/tests/protocol2_0/broadcast_ping.py @@ -90,9 +90,9 @@ def getch(): if dxl_comm_result != COMM_SUCCESS: print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) -print "Detected Dynamixel :" +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() \ No newline at end of file +portHandler.closePort() diff --git a/python/tests/protocol2_0/bulk_read_write.py b/python/tests/protocol2_0/bulk_read_write.py index ca126484..651f32fb 100644 --- a/python/tests/protocol2_0/bulk_read_write.py +++ b/python/tests/protocol2_0/bulk_read_write.py @@ -164,11 +164,11 @@ def getch(): 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 packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) # Clear bulkwrite parameter storage groupBulkWrite.clearParam() @@ -177,7 +177,7 @@ def getch(): # Bulkread present position and LED status dxl_comm_result = groupBulkRead.txRxPacket() if dxl_comm_result != COMM_SUCCESS: - print packetHandler.getTxRxResult(dxl_comm_result) + 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) @@ -206,7 +206,7 @@ def getch(): if index == 0: index = 1 else: - index = 0 + index = 0 # Clear bulkread parameter storage groupBulkRead.clearParam() @@ -226,4 +226,4 @@ def getch(): print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() diff --git a/python/tests/protocol2_0/indirect_address.py b/python/tests/protocol2_0/indirect_address.py index 3e259fb4..52013007 100644 --- a/python/tests/protocol2_0/indirect_address.py +++ b/python/tests/protocol2_0/indirect_address.py @@ -187,7 +187,7 @@ def getch(): 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: @@ -220,7 +220,7 @@ def getch(): # Syncwrite all dxl_comm_result = groupSyncWrite.txPacket() if dxl_comm_result != COMM_SUCCESS: - print packetHandler.getTxRxResult(dxl_comm_result) + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) # Clear syncwrite parameter storage groupSyncWrite.clearParam() @@ -229,7 +229,7 @@ def getch(): # Syncread present position from indirectdata2 dxl_comm_result = groupSyncRead.txRxPacket() if dxl_comm_result != COMM_SUCCESS: - print packetHandler.getTxRxResult(dxl_comm_result) + 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) @@ -258,7 +258,7 @@ def getch(): if index == 0: index = 1 else: - index = 0 + index = 0 # Clear syncread parameter storage groupSyncRead.clearParam() @@ -271,4 +271,4 @@ def getch(): print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() diff --git a/python/tests/protocol2_0/reboot.py b/python/tests/protocol2_0/reboot.py index 724d3f0a..70296e0f 100644 --- a/python/tests/protocol2_0/reboot.py +++ b/python/tests/protocol2_0/reboot.py @@ -103,4 +103,4 @@ def getch(): # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() diff --git a/python/tests/protocol2_0/sync_read_write.py b/python/tests/protocol2_0/sync_read_write.py index 3acab67c..3636162e 100644 --- a/python/tests/protocol2_0/sync_read_write.py +++ b/python/tests/protocol2_0/sync_read_write.py @@ -204,7 +204,7 @@ def getch(): if index == 0: index = 1 else: - index = 0 + index = 0 # Clear syncread parameter storage groupSyncRead.clearParam() @@ -224,4 +224,4 @@ def getch(): print("%s" % packetHandler.getRxPacketError(dxl_error)) # Close port -portHandler.closePort() \ No newline at end of file +portHandler.closePort() From cb586cbe1ff066e1d92a0570358ba54dba80b249 Mon Sep 17 00:00:00 2001 From: Leon Jung Date: Thu, 26 Apr 2018 14:38:59 +0900 Subject: [PATCH 10/25] added: DynamixelSDK Python as a native language | Alpha | Ubuntu 16.04 --- python/LICENSE.txt | 201 +++++ python/README.txt | 1 + .../dynamixel_functions.py | 173 ---- python/protocol1_0/bulk_read.py | 241 ------ python/protocol1_0/factory_reset.py | 185 ---- python/protocol1_0/multi_port.py | 233 ----- python/protocol1_0/ping.py | 108 --- python/protocol1_0/read_write.py | 172 ---- python/protocol1_0/sync_write.py | 222 ----- python/protocol2_0/broadcast_ping.py | 107 --- python/protocol2_0/bulk_read_write.py | 248 ------ python/protocol2_0/factory_reset.py | 186 ---- python/protocol2_0/indirect_address.py | 321 ------- python/protocol2_0/multi_port.py | 233 ----- python/protocol2_0/read_write.py | 172 ---- python/protocol2_0/rebooting.py | 112 --- python/protocol2_0/sync_read_write.py | 246 ------ python/protocol_combined/protocol_combined.py | 226 ----- python/setup.py | 15 + python/src/dynamixel_sdk/__init__.py | 27 + python/src/dynamixel_sdk/group_bulk_read.py | 143 ++++ python/src/dynamixel_sdk/group_bulk_write.py | 108 +++ python/src/dynamixel_sdk/group_sync_read.py | 141 ++++ python/src/dynamixel_sdk/group_sync_write.py | 92 ++ python/src/dynamixel_sdk/packet_handler.py | 31 + python/src/dynamixel_sdk/port_handler.py | 38 + .../src/dynamixel_sdk/port_handler_linux.py | 173 ++++ python/src/dynamixel_sdk/port_handler_mac.py | 201 +++++ .../src/dynamixel_sdk/port_handler_windows.py | 201 +++++ .../dynamixel_sdk/protocol1_packet_handler.py | 551 ++++++++++++ .../dynamixel_sdk/protocol2_packet_handler.py | 793 ++++++++++++++++++ python/src/dynamixel_sdk/robotis_def.py | 68 ++ python/tests/protocol1_0/bulk_read.py | 216 +++++ python/tests/protocol1_0/factory_reset.py | 168 ++++ python/tests/protocol1_0/multi_port.py | 206 +++++ python/tests/protocol1_0/ping.py | 100 +++ python/tests/protocol1_0/read_write.py | 153 ++++ python/tests/protocol1_0/sync_write.py | 201 +++++ python/tests/protocol2_0/broadcast_ping.py | 98 +++ python/tests/protocol2_0/bulk_read_write.py | 229 +++++ python/tests/protocol2_0/factory_reset.py | 170 ++++ python/tests/protocol2_0/indirect_address.py | 274 ++++++ python/tests/protocol2_0/multi_port.py | 206 +++++ python/{ => tests}/protocol2_0/ping.py | 59 +- python/tests/protocol2_0/read_write.py | 154 ++++ python/tests/protocol2_0/reboot.py | 106 +++ python/tests/protocol2_0/sync_read_write.py | 227 +++++ python/tests/protocol_combined.py | 199 +++++ 48 files changed, 5517 insertions(+), 3218 deletions(-) create mode 100644 python/LICENSE.txt create mode 100644 python/README.txt delete mode 100644 python/dynamixel_functions_py/dynamixel_functions.py delete mode 100644 python/protocol1_0/bulk_read.py delete mode 100644 python/protocol1_0/factory_reset.py delete mode 100644 python/protocol1_0/multi_port.py delete mode 100644 python/protocol1_0/ping.py delete mode 100644 python/protocol1_0/read_write.py delete mode 100644 python/protocol1_0/sync_write.py delete mode 100644 python/protocol2_0/broadcast_ping.py delete mode 100644 python/protocol2_0/bulk_read_write.py delete mode 100644 python/protocol2_0/factory_reset.py delete mode 100644 python/protocol2_0/indirect_address.py delete mode 100644 python/protocol2_0/multi_port.py delete mode 100644 python/protocol2_0/read_write.py delete mode 100644 python/protocol2_0/rebooting.py delete mode 100644 python/protocol2_0/sync_read_write.py delete mode 100644 python/protocol_combined/protocol_combined.py create mode 100644 python/setup.py create mode 100644 python/src/dynamixel_sdk/__init__.py create mode 100644 python/src/dynamixel_sdk/group_bulk_read.py create mode 100644 python/src/dynamixel_sdk/group_bulk_write.py create mode 100644 python/src/dynamixel_sdk/group_sync_read.py create mode 100644 python/src/dynamixel_sdk/group_sync_write.py create mode 100644 python/src/dynamixel_sdk/packet_handler.py create mode 100644 python/src/dynamixel_sdk/port_handler.py create mode 100644 python/src/dynamixel_sdk/port_handler_linux.py create mode 100644 python/src/dynamixel_sdk/port_handler_mac.py create mode 100644 python/src/dynamixel_sdk/port_handler_windows.py create mode 100644 python/src/dynamixel_sdk/protocol1_packet_handler.py create mode 100644 python/src/dynamixel_sdk/protocol2_packet_handler.py create mode 100644 python/src/dynamixel_sdk/robotis_def.py create mode 100644 python/tests/protocol1_0/bulk_read.py create mode 100644 python/tests/protocol1_0/factory_reset.py create mode 100644 python/tests/protocol1_0/multi_port.py create mode 100644 python/tests/protocol1_0/ping.py create mode 100644 python/tests/protocol1_0/read_write.py create mode 100644 python/tests/protocol1_0/sync_write.py create mode 100644 python/tests/protocol2_0/broadcast_ping.py create mode 100644 python/tests/protocol2_0/bulk_read_write.py create mode 100644 python/tests/protocol2_0/factory_reset.py create mode 100644 python/tests/protocol2_0/indirect_address.py create mode 100644 python/tests/protocol2_0/multi_port.py rename python/{ => tests}/protocol2_0/ping.py (52%) create mode 100644 python/tests/protocol2_0/read_write.py create mode 100644 python/tests/protocol2_0/reboot.py create mode 100644 python/tests/protocol2_0/sync_read_write.py create mode 100644 python/tests/protocol_combined.py 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/ping.py b/python/protocol1_0/ping.py deleted file mode 100644 index 75f5c397..00000000 --- a/python/protocol1_0/ping.py +++ /dev/null @@ -1,108 +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 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 - -# 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-*" - -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 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) -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] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number)) - -# Close port -dynamixel.closePort(port_num) 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/rebooting.py b/python/protocol2_0/rebooting.py deleted file mode 100644 index 4e809222..00000000 --- a/python/protocol2_0/rebooting.py +++ /dev/null @@ -1,112 +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) - -# -# ********* reboot 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 - -# 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-*" - -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() - -# Trigger -print("Press any key to reboot") -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) -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] reboot Succeeded" % (DXL_ID)) - -# 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..2fd15e5e --- /dev/null +++ b/python/setup.py @@ -0,0 +1,15 @@ + +from setuptools import setup, find_packages + +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' +) diff --git a/python/src/dynamixel_sdk/__init__.py b/python/src/dynamixel_sdk/__init__.py new file mode 100644 index 00000000..b5d22c54 --- /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 * \ No newline at end of file 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..b1da0f5e --- /dev/null +++ b/python/src/dynamixel_sdk/group_bulk_read.py @@ -0,0 +1,143 @@ +#!/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 id in self.data_dict: + if self.ph.getProtocolVersion() == 1.0: + self.param.append(self.data_dict[id][2]) # LEN + self.param.append(id) # ID + self.param.append(self.data_dict[id][1]) # ADDR + else: + self.param.append(id) # ID + self.param.append(DXL_LOBYTE(self.data_dict[id][1])) # ADDR_L + self.param.append(DXL_HIBYTE(self.data_dict[id][1])) # ADDR_H + self.param.append(DXL_LOBYTE(self.data_dict[id][2])) # LEN_L + self.param.append(DXL_HIBYTE(self.data_dict[id][2])) # LEN_H + + def addParam(self, id, start_address, data_length): + if id in self.data_dict: # id already exist + return False + + data = [] # [0] * data_length + self.data_dict[id] = [data, start_address, data_length] + + self.is_param_changed = True + return True + + def removeParam(self, id): + if not id in self.data_dict: # NOT exist + return + + del self.data_dict[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 == 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 id in self.data_dict: + self.data_dict[id][PARAM_NUM_DATA], result, _ = self.ph.readRx(self.port, id, self.data_dict[id][PARAM_NUM_LENGTH], self.data_dict[id][PARAM_NUM_DATA]) + if result != COMM_SUCCESS: + return result + + if result == COMM_SUCCESS: + self.last_result = True + + return result + + def txRxPacket(self): + result = COMM_TX_FAIL + + result = self.txPacket() + if result != COMM_SUCCESS: + return result + + return self.rxPacket() + + def isAvailable(self, id, address, data_length): + if self.last_result == False or not id in self.data_dict: + return False + + start_addr = self.data_dict[id][PARAM_NUM_ADDRESS] + + if (address < start_addr) or (start_addr + self.data_dict[id][PARAM_NUM_LENGTH] - data_length < address): + return False + + return True + + def getData(self, id, address, data_length): + if self.isAvailable(id, address, data_length) == False: + return 0 + + start_addr = self.data_dict[id][PARAM_NUM_ADDRESS] + + if data_length == 1: + return self.data_dict[id][PARAM_NUM_DATA][address - start_addr] + elif data_length == 2: + return DXL_MAKEWORD(self.data_dict[id][PARAM_NUM_DATA][address - start_addr], self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 1]) + elif data_length == 4: + return DXL_MAKEDWORD(DXL_MAKEWORD(self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 0], self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 1]), DXL_MAKEWORD(self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 2], self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 3])) + else: + return 0 \ No newline at end of file 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..a28e05c1 --- /dev/null +++ b/python/src/dynamixel_sdk/group_bulk_write.py @@ -0,0 +1,108 @@ +#!/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 id in self.data_list: + if not self.data_list[id]: + return + + self.param.append(id) + self.param.append(DXL_LOBYTE(self.data_list[id][1])) + self.param.append(DXL_HIBYTE(self.data_list[id][1])) + self.param.append(DXL_LOBYTE(self.data_list[id][2])) + self.param.append(DXL_HIBYTE(self.data_list[id][2])) + + self.param.extend(self.data_list[id][0]) + + def addParam(self, id, start_address, data_length, data): + if self.ph.getProtocolVersion() == 1.0: + return False + + if id in self.data_list: # id already exist + return False + + if len(data) > data_length: # input data is longer than set + return False + + self.data_list[id] = [data, start_address, data_length] + + self.is_param_changed = True + return True + + def removeParam(self, id): + if self.ph.getProtocolVersion() == 1.0: + return + + if not id in self.data_list: # NOT exist + return + + del self.data_list[id] + + self.is_param_changed = True + + def changeParam(self, id, start_address, data_length, data): + if self.ph.getProtocolVersion() == 1.0: + return False + + if not id in self.data_list: # NOT exist + return False + + if len(data) > data_length: # input data is longer than set + return False + + self.data_list[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 == True or len(self.param) == 0: + self.makeParam() + + return self.ph.bulkWriteTxOnly(self.port, self.param, len(self.param)) \ No newline at end of file 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..758fdee2 --- /dev/null +++ b/python/src/dynamixel_sdk/group_sync_read.py @@ -0,0 +1,141 @@ +#!/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 id in self.data_dict: + self.param.append(id) + + def addParam(self, id): + if self.ph.getProtocolVersion() == 1.0: + return False + + if id in self.data_dict: # id already exist + return False + + self.data_dict[id] = [] # [0] * self.data_length + + self.is_param_changed = True + return True + + def removeParam(self, id): + if self.ph.getProtocolVersion() == 1.0: + return + + if not id in self.data_dict: # NOT exist + return + + del self.data_dict[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 == 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 id in self.data_dict: + self.data_dict[id], result, _ = self.ph.readRx(self.port, id, self.data_length, self.data_dict[id]) + 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 = COMM_TX_FAIL + + result = self.txPacket() + if result != COMM_SUCCESS: + return result + + return self.rxPacket() + + def isAvailable(self, id, address, data_length): + if self.ph.getProtocolVersion() == 1.0 or self.last_result == False or not id 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, id, address, data_length): + if self.isAvailable(id, address, data_length) == False: + return 0 + + if data_length == 1: + return self.data_dict[id][address - self.start_address] + elif data_length == 2: + return DXL_MAKEWORD(self.data_dict[id][address - self.start_address], self.data_dict[id][address - self.start_address + 1]) + elif data_length == 4: + return DXL_MAKEDWORD(DXL_MAKEWORD(self.data_dict[id][address - self.start_address + 0], self.data_dict[id][address - self.start_address + 1]), DXL_MAKEWORD(self.data_dict[id][address - self.start_address + 2], self.data_dict[id][address - self.start_address + 3])) + else: + return 0 \ No newline at end of file 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..75b7b968 --- /dev/null +++ b/python/src/dynamixel_sdk/group_sync_write.py @@ -0,0 +1,92 @@ +#!/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 id in self.data_dict: + if not self.data_dict[id]: + return + + self.param.append(id) + self.param.extend(self.data_dict[id]) + + def addParam(self, id, data): + if (id in self.data_dict): # id already exist + return False + + if len(data) > self.data_length: # input data is longer than set + return False + + self.data_dict[id] = data + + self.is_param_changed = True + return True + + def removeParam(self, id): + if not id in self.data_dict: # NOT exist + return + + del self.data_dict[id] + + self.is_param_changed = True + + def changeParam(self, id, data): + if not id 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[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 == 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)) \ No newline at end of file diff --git a/python/src/dynamixel_sdk/packet_handler.py b/python/src/dynamixel_sdk/packet_handler.py new file mode 100644 index 00000000..84617d4f --- /dev/null +++ b/python/src/dynamixel_sdk/packet_handler.py @@ -0,0 +1,31 @@ +#!/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): + if protocol_version == 1.0: + return Protocol1PacketHandler() + elif protocol_version == 2.0: + return Protocol2PacketHandler() + else: + return Protocol2PacketHandler() \ No newline at end of file diff --git a/python/src/dynamixel_sdk/port_handler.py b/python/src/dynamixel_sdk/port_handler.py new file mode 100644 index 00000000..d542ba90 --- /dev/null +++ b/python/src/dynamixel_sdk/port_handler.py @@ -0,0 +1,38 @@ +#!/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 platform + +if platform.system() == 'Linux': + from port_handler_linux import * + + class PortHandler(PortHandlerLinux): + pass +elif platform.system() == 'Windows': + from port_handler_windows import * + + class PortHandler(PortHandlerWindows): + pass +elif platform.system() == 'Darwin': + from port_handler_mac import * + + class PortHandler(PortHandlerMac): + pass diff --git a/python/src/dynamixel_sdk/port_handler_linux.py b/python/src/dynamixel_sdk/port_handler_linux.py new file mode 100644 index 00000000..0adee5b3 --- /dev/null +++ b/python/src/dynamixel_sdk/port_handler_linux.py @@ -0,0 +1,173 @@ +#!/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 + +LATENCY_TIMER = 4 #16 +DEFAULT_BAUDRATE = 1000000 + +class PortHandlerLinux(object): + def __init__(self, port_name): + self.socket_fd = 0.0 + 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.setPortName(port_name) + + def openPort(self): + return self.setBaudRate(self.baudrate) + + def closePort(self): + self.ser.close() + + 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) + + # When the port is already open, it will be closed and reopened with the new setting. + # self.closePort() + + if baud <= 0: + # self.setupPort(38400) + # self.baudrate = baudrate + return False + 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): + # read_bytes = self.ser.read(length) + read_bytes = [] + + read_bytes.extend([ord(ch) for ch in self.ser.read(length)]) + # for i in range(0, len(read_bytes)): + # read_bytes[i] = int.frombytes(read_bytes[i], byteorder = 'little') + + # for i in read_bytes: + # print i + return read_bytes + + def writePort(self, packet): + # for i in packet: + # print i + + return self.ser.write(packet) + + def setPacketTimeout(self, packet_length): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = (self.tx_time_per_byte * self.packet_length) + (LATENCY_TIMER * 2.0) + 2.0 + + def setPacketTimeout(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): + millis = round(time.time() * 1000000000) / 1000000.0 + # print "%.6f" % millis + return millis + + def getTimeSinceStart(self): + time = self.getCurrentTime() - self.packet_start_time + if time < 0.0: + packet_start_time = self.getCurrentTime() + + return time + + def setupPort(self, cflag_baud): + self.ser = serial.Serial( + port = self.port_name, + baudrate = self.baudrate, + # parity = serial.PARITY_ODD, + # stopbits = serial.STOPBITS_TWO, + bytesize = serial.EIGHTBITS #bytesize = serial.SEVENBITS + , timeout = 0 + ) + + self.ser.reset_input_buffer() + + self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 + + return True + + def getCFlagBaud(self, baudrate): + if baudrate == 9600: + return 9600 + elif baudrate == 19200: + return 19200 + elif baudrate == 38400: + return 38400 + elif baudrate == 57600: + return 57600 + elif baudrate == 115200: + return 115200 + elif baudrate == 230400: + return 230400 + elif baudrate == 460800: + return 460800 + elif baudrate == 500000: + return 500000 + elif baudrate == 576000: + return 576000 + elif baudrate == 921600: + return 921600 + elif baudrate == 1000000: + return 1000000 + elif baudrate == 1152000: + return 1152000 + elif baudrate == 1500000: + return 1500000 + elif baudrate == 2000000: + return 2000000 + elif baudrate == 2500000: + return 2500000 + elif baudrate == 3000000: + return 3000000 + elif baudrate == 3500000: + return 3500000 + elif baudrate == 4000000: + return 4000000 + else: + return -1 \ No newline at end of file diff --git a/python/src/dynamixel_sdk/port_handler_mac.py b/python/src/dynamixel_sdk/port_handler_mac.py new file mode 100644 index 00000000..4ae0c6a8 --- /dev/null +++ b/python/src/dynamixel_sdk/port_handler_mac.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) + +import time +import serial + +LATENCY_TIMER = 16 +DEFAULT_BAUDRATE = 1000000 + +class PortHandlerMac(object): + def __init__(self, port_name): + self.socket_fd = 0.0 + 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.setPortName(port_name) + + def openPort(self): + return self.setBaudRate(self.baudrate) + + def closePort(self): + self.ser.close() + + 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) + + self.closePort() + + if baud <= 0: + setupPort(38400) + self.baudrate = baudrate + return setCustomBaudrate(baudrate) + else: + self.baudrate = baudrate + return setupPort(baud) + + # TODO:more simplify + + def getBaudRate(self): + return self.baudrate + + def getBytesAvailable(self): + pass + + # TODO: fill + + def readPort(self, packet, length): + pass + + # TODO: fill + + def writePort(self, packet): + ser.write(packet) + + +# ser.isOpen() + +# print 'Enter your commands below.\r\nInsert "exit" to leave the application.' + +# input=1 +# while 1 : +# # get keyboard input +# input = raw_input(">> ") +# # Python 3 users +# # input = input(">> ") +# if input == 'exit': +# ser.close() +# exit() +# else: +# # send the character to the device +# # (note that I happend a \r\n carriage return and line feed to the characters - this is requested by my device) +# ser.write(input) +# # out = '' +# # # let's wait one second before reading output (let's give device time to answer) +# # time.sleep(1) +# # while ser.inWaiting() > 0: +# # out += ser.read(1) + +# # if out != '': +# # print ">>" + out + + + + + def setPacketTimeout(self, packet_length): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = (self.tx_time_per_byte * self.packet_length) + (LATENCY_TIMER * 2.0) + 2.0 + + def setPacketTimeout(self, msec): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = msec + + def isPacketTimeout(self): + if self.getTimeSinceStart() > packet_timeout: + self.packet_timeout = 0 + return True + + return False + + def getCurrentTime(self): + pass + + #TODO : fill + + def getTimeSinceStart(self): + time = self.getCurrentTime() - self.packet_start_time + if time < 0.0: + packet_start_time = self.getCurrentTime() + + return time + + def setupPort(self, cflag_baud): + + # TODO: Error exception + + self.ser = serial.Serial( + port = self.port_name, #'/dev/ttyUSB0', + baudrate = self.baudrate, # 1000000, + parity = serial.PARITY_ODD, + stopbits = serial.STOPBITS_TWO, + bytesize = serial.SEVENBITS + ) + + self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 + + return True + + def setCustomBaudrate(self, speed): + pass + + #TODO: fill + + def getCFlagBaud(self, baudrate): + if baudrate is 9600: + return 9600 + elif baudrate is 19200: + return 19200 + elif baudrate is 38400: + return 38400 + elif baudrate is 57600: + return 57600 + elif baudrate is 115200: + return 115200 + elif baudrate is 230400: + return 230400 + elif baudrate is 460800: + return 460800 + elif baudrate is 500000: + return 500000 + elif baudrate is 576000: + return 576000 + elif baudrate is 921600: + return 921600 + elif baudrate is 1000000: + return 1000000 + elif baudrate is 1152000: + return 1152000 + elif baudrate is 1500000: + return 1500000 + elif baudrate is 2000000: + return 2000000 + elif baudrate is 2500000: + return 2500000 + elif baudrate is 3000000: + return 3000000 + elif baudrate is 3500000: + return 3500000 + elif baudrate is 4000000: + return 4000000 + else: + return -1 \ No newline at end of file diff --git a/python/src/dynamixel_sdk/port_handler_windows.py b/python/src/dynamixel_sdk/port_handler_windows.py new file mode 100644 index 00000000..95a92d46 --- /dev/null +++ b/python/src/dynamixel_sdk/port_handler_windows.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) + +import time +import serial + +LATENCY_TIMER = 16 +DEFAULT_BAUDRATE = 1000000 + +class PortHandlerWindows(object): + def __init__(self, port_name): + self.socket_fd = 0.0 + 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.setPortName(port_name) + + def openPort(self): + return self.setBaudRate(self.baudrate) + + def closePort(self): + self.ser.close() + + 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) + + self.closePort() + + if baud <= 0: + setupPort(38400) + self.baudrate = baudrate + return setCustomBaudrate(baudrate) + else: + self.baudrate = baudrate + return setupPort(baud) + + # TODO:more simplify + + def getBaudRate(self): + return self.baudrate + + def getBytesAvailable(self): + pass + + # TODO: fill + + def readPort(self, packet, length): + pass + + # TODO: fill + + def writePort(self, packet): + ser.write(packet) + + +# ser.isOpen() + +# print 'Enter your commands below.\r\nInsert "exit" to leave the application.' + +# input=1 +# while 1 : +# # get keyboard input +# input = raw_input(">> ") +# # Python 3 users +# # input = input(">> ") +# if input == 'exit': +# ser.close() +# exit() +# else: +# # send the character to the device +# # (note that I happend a \r\n carriage return and line feed to the characters - this is requested by my device) +# ser.write(input) +# # out = '' +# # # let's wait one second before reading output (let's give device time to answer) +# # time.sleep(1) +# # while ser.inWaiting() > 0: +# # out += ser.read(1) + +# # if out != '': +# # print ">>" + out + + + + + def setPacketTimeout(self, packet_length): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = (self.tx_time_per_byte * self.packet_length) + (LATENCY_TIMER * 2.0) + 2.0 + + def setPacketTimeout(self, msec): + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = msec + + def isPacketTimeout(self): + if self.getTimeSinceStart() > packet_timeout: + self.packet_timeout = 0 + return True + + return False + + def getCurrentTime(self): + pass + + #TODO : fill + + def getTimeSinceStart(self): + time = self.getCurrentTime() - self.packet_start_time + if time < 0.0: + packet_start_time = self.getCurrentTime() + + return time + + def setupPort(self, cflag_baud): + + # TODO: Error exception + + self.ser = serial.Serial( + port = self.port_name, #'/dev/ttyUSB0', + baudrate = self.baudrate, # 1000000, + parity = serial.PARITY_ODD, + stopbits = serial.STOPBITS_TWO, + bytesize = serial.SEVENBITS + ) + + self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0 + + return True + + def setCustomBaudrate(self, speed): + pass + + #TODO: fill + + def getCFlagBaud(self, baudrate): + if baudrate is 9600: + return 9600 + elif baudrate is 19200: + return 19200 + elif baudrate is 38400: + return 38400 + elif baudrate is 57600: + return 57600 + elif baudrate is 115200: + return 115200 + elif baudrate is 230400: + return 230400 + elif baudrate is 460800: + return 460800 + elif baudrate is 500000: + return 500000 + elif baudrate is 576000: + return 576000 + elif baudrate is 921600: + return 921600 + elif baudrate is 1000000: + return 1000000 + elif baudrate is 1152000: + return 1152000 + elif baudrate is 1500000: + return 1500000 + elif baudrate is 2000000: + return 2000000 + elif baudrate is 2500000: + return 2500000 + elif baudrate is 3000000: + return 3000000 + elif baudrate is 3500000: + return 3500000 + elif baudrate is 4000000: + return 4000000 + else: + return -1 \ No newline at end of file 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..134f082f --- /dev/null +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -0,0 +1,551 @@ +#!/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 + + # 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 += 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() == True: + if rx_length == 0: + result = COMM_RX_TIMEOUT + print "COMM_RX_TIMEOUT 1" + else: + result = COMM_RX_CORRUPT + print "COMM_RX_CORRUPT 1" + 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 + print "COMM_RX_CORRUPT 2" + + print("checksum / rxpacket : ", checksum, rxpacket[wait_length - 1]) + + break + + else: + #remove unnecessary packets + del rxpacket[0 : idx] + rx_length -= idx + + else: + # check timeout + if port.isPacketTimeout() == True: + if rx_length == 0: + result = COMM_RX_TIMEOUT + print "COMM_RX_TIMEOUT 3" + else: + result = COMM_RX_CORRUPT + print "COMM_RX_CORRUPT 3" + break + + port.is_using = False + + return rxpacket, result + + # NOT for BulkRead + def txRxPacket(self, port, txpacket, rxpacket=[], error=0): + result = COMM_TX_FAIL + + # 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 + # (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(txpacket[PKT_PARAMETER0 + 1] + 6) + else: + port.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + + # rx packet + while True: + rxpacket, result = self.rxPacket(port, rxpacket) + + if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: + break + + if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: + if error != 0: + error = rxpacket[PKT_ERROR] + + return rxpacket, result, error + + def ping(self, port, id, model_number=0, error=0): + result = COMM_TX_FAIL + + txpacket = [0] * 6 + rxpacket = [] + + if id >= BROADCAST_ID: + return model_number, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_PING + + rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket, error) + + if result == COMM_SUCCESS: + data_read, result, error = self.readTxRx(port, 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={}): + return data_list, COMM_NOT_AVAILABLE + + def action(self, port, id): + txpacket = [0] * 6 + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_ACTION + + _, result, _ = self.txRxPacket(port, txpacket) + + return result + + def reboot(self, port, id, error=0): + return COMM_NOT_AVAILABLE, error + + def factoryReset(self, port, id, option, error=0): + txpacket = [0] * 6 + rxpacket = [] + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET + + _, result, error = self.txRxPacket(port, txpacket, rxpacket, error) + return result, error + + def readTx(self, port, id, address, length): + result = COMM_TX_FAIL + + txpacket = [0] * 8 + + if id >= BROADCAST_ID: + return COMM_NOT_AVAILABLE + + txpacket[PKT_ID] = 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, id, length, data=[], error=0): + result = COMM_TX_FAIL + + rxpacket = [] + + if len(data) != 0: + data = [] + + while True: + rxpacket, result = self.rxPacket(port, rxpacket) + + if result != COMM_SUCCESS or rxpacket[PKT_ID] == id: + break + + if result == COMM_SUCCESS and rxpacket[PKT_ID] == id: + if error != 0: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) + + del rxpacket[:]; del rxpacket + return data, result, error + + def readTxRx(self, port, id, address, length, data=[], error=0): + result = COMM_TX_FAIL + txpacket = [0] * 8 + rxpacket = [] + + if len(data) != 0: + data = [] + + if id >= BROADCAST_ID: + return data, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = 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, rxpacket, error) + if result == COMM_SUCCESS: + if error != 0: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) + + del rxpacket[:]; del rxpacket + return data, result, error + + def read1ByteTx(self, port, id, address): + return self.readTx(port, id, address, 1) + def read1ByteRx(self, port, id, data=[], error=0): + data_read = [0] + data_read, result, error = self.readRx(port, id, 1, data_read, error) + if result == COMM_SUCCESS: + data = data_read[0] + return data, result, error + def read1ByteTxRx(self, port, id, address, data=[], error=0): + data_read = [0] + data_read, result, error = self.readTxRx(port, id, address, 1, data_read, error) + if result == COMM_SUCCESS: + data = data_read[0] + return data, result, error + + def read2ByteTx(self, port, id, address): + return self.readTx(port, id, address, 2) + def read2ByteRx(self, port, id, data=[], error=0): + data_read = [0] * 2 + data_read, result, error = self.readRx(port, id, 2, data_read, error) + if result == COMM_SUCCESS: + data = DXL_MAKEWORD(data_read[0], data_read[1]) + return data, result, error + def read2ByteTxRx(self, port, id, address, data=[], error=0): + data_read = [0] * 2 + data_read, result, error = self.readTxRx(port, id, address, 2, data_read, error) + if result == COMM_SUCCESS: + data = DXL_MAKEWORD(data_read[0], data_read[1]) + return data, result, error + + def read4ByteTx(self, port, id, address): + return self.readTx(port, id, address, 4) + def read4ByteRx(self, port, id, data=[], error=0): + data_read = [0] * 4 + data_read, result, error = self.readRx(port, id, 4, data_read, error) + if result == COMM_SUCCESS: + data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) + return data, result, error + def read4ByteTxRx(self, port, id, address, data=[], error=0): + data_read = [0] * 4 + data_read, result, error = self.readTxRx(port, id, address, 4, data_read, error) + if result == COMM_SUCCESS: + data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) + return data, result, error + + def writeTxOnly(self, port, id, address, length, data): + result = COMM_TX_FAIL + + txpacket = [0] * (length + 7) + + txpacket[PKT_ID] = 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 + + del txpacket[:]; del txpacket + return result + + def writeTxRx(self, port, id, address, length, data, error=0): + result = COMM_TX_FAIL + + txpacket = [0] * (length + 7) + rxpacket = [] + + txpacket[PKT_ID] = 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, rxpacket, error) + + del txpacket[:]; del txpacket + return result, error + + def write1ByteTxOnly(self, port, id, address, data): + data_write = [data] + return self.writeTxOnly(port, id, address, 1, data_write) + def write1ByteTxRx(self, port, id, address, data, error=0): + data_write = [data] + return self.writeTxRx(port, id, address, 1, data_write) + + def write2ByteTxOnly(self, port, id, address, data): + data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] + return self.writeTxOnly(port, id, address, 2, data_write) + def write2ByteTxRx(self, port, id, address, data, error=0): + data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] + return self.writeTxRx(port, id, address, 2, data_write) + + def write4ByteTxOnly(self, port, 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, id, address, 4, data_write) + def write4ByteTxRx(self, port, id, address, data, error=0): + 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, id, address, 4, data_write) + + + def regWriteTxOnly(self, port, id, address, length, data): + result = COMM_TX_FAIL + + txpacket = [0] * (length + 6) + + txpacket[PKT_ID] = 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 + + del txpacket[:]; del txpacket + return result + + def regWriteTxRx(self, port, id, address, length, data, error=0): + result = COMM_TX_FAIL + + txpacket = [0] * (length + 6) + rxpacket = [] + + txpacket[PKT_ID] = 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, rxpacket, error) + + del txpacket[:]; del 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): + result = COMM_TX_FAIL + + 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) + + del txpacket[:]; del txpacket + return result + + def bulkReadTx(self, port, param, param_length): + result = COMM_TX_FAIL + + 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) + + del txpacket[:]; del txpacket + return result + + def bulkWriteTxOnly(self, port, param, param_length): + return COMM_NOT_AVAILABLE \ No newline at end of file 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..57bd0f52 --- /dev/null +++ b/python/src/dynamixel_sdk/protocol2_packet_handler.py @@ -0,0 +1,793 @@ +#!/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 + + temp[PKT_HEADER0 : PKT_HEADER0 + PKT_LENGTH_H + 1] = packet[PKT_HEADER0 : PKT_HEADER0 + PKT_LENGTH_H + 1] # FF FF FD XX ID LEN_L LEN_H + + 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 += 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() == True: + 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 = rx_length - idx + + else: + if port.isPacketTimeout() == True: + 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=[], error=0): + result = COMM_TX_FAIL + + # 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, rxpacket) + + if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: + break + + if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: + if error != 0: + error = rxpacket[PKT_ERROR] + + return rxpacket, result, error + + def ping(self, port, id, model_number=0, error=0): + result = COMM_TX_FAIL + + txpacket = [0] * 10 + rxpacket = [] + + if id >= BROADCAST_ID: + return model_number, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH_L] = 3 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_PING + + rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket, error) + 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 + result = COMM_TX_FAIL + + 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() == True: # 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 + + return data_list, result + + def action(self, port, id): + txpacket = [0] * 10 + + txpacket[PKT_ID] = 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, id, error=0): + txpacket = [0] * 10 + rxpacket = [] + + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH_L] = 3 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_REBOOT + + rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket, error) + return result, error + + def factoryReset(self, port, id, option, error=0): + txpacket = [0] * 11 + rxpacket = [] + + txpacket[PKT_ID] = 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, rxpacket, error) + return result, error + + def readTx(self, port, id, address, length): + result = COMM_TX_FAIL + + txpacket = [0] * 14 + + if id >= BROADCAST_ID: + return COMM_NOT_AVAILABLE + + txpacket[PKT_ID] = 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, id, length, data=[], error=0): + result = COMM_TX_FAIL + + rxpacket = [] + + if len(data) != 0: + data = [] + + while True: + rxpacket, result = self.rxPacket(port, rxpacket) + + if result != COMM_SUCCESS or rxpacket[PKT_ID] == id: + break + + if result == COMM_SUCCESS and rxpacket[PKT_ID] == id: + if error != 0: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length]) + + del rxpacket[:]; del rxpacket + return data, result, error + + def readTxRx(self, port, id, address, length, data=[], error=0): + result = COMM_TX_FAIL + + txpacket = [0] * 14 + rxpacket = [] + + if len(data) != 0: + data = [] + + if id >= BROADCAST_ID: + return data, COMM_NOT_AVAILABLE, error + + txpacket[PKT_ID] = 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, rxpacket, error) + if result == COMM_SUCCESS: + if error != 0: + error = rxpacket[PKT_ERROR] + + data.extend(rxpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length]) + + del rxpacket[:]; del rxpacket + return data, result, error + + def read1ByteTx(self, port, id, address): + return self.readTx(port, id, address, 1) + def read1ByteRx(self, port, id, data=[], error=0): + data_read = [0] + data_read, result, error = self.readRx(port, id, 1, data_read, error) + if result == COMM_SUCCESS: + data = data_read[0] + return data, result, error + def read1ByteTxRx(self, port, id, address, data=[], error=0): + data_read = [0] + data_read, result, error = self.readTxRx(port, id, address, 1, data_read, error) + if result == COMM_SUCCESS: + data = data_read[0] + return data, result, error + + def read2ByteTx(self, port, id, address): + return self.readTx(port, id, address, 2) + def read2ByteRx(self, port, id, data=[], error=0): + data_read = [0] * 2 + data_read, result, error = self.readRx(port, id, 2, data_read, error) + if result == COMM_SUCCESS: + data = DXL_MAKEWORD(data_read[0], data_read[1]) + return data, result, error + def read2ByteTxRx(self, port, id, address, data=[], error=0): + data_read = [0] * 2 + data_read, result, error = self.readTxRx(port, id, address, 2, data_read, error) + if result == COMM_SUCCESS: + data = DXL_MAKEWORD(data_read[0], data_read[1]) + return data, result, error + + def read4ByteTx(self, port, id, address): + return self.readTx(port, id, address, 4) + def read4ByteRx(self, port, id, data=[], error=0): + data_read = [0] * 4 + data_read, result, error = self.readRx(port, id, 4, data_read, error) + if result == COMM_SUCCESS: + data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) + return data, result, error + def read4ByteTxRx(self, port, id, address, data=[], error=0): + data_read = [0] * 4 + data_read, result, error = self.readTxRx(port, id, address, 4, data_read, error) + if result == COMM_SUCCESS: + data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) + return data, result, error + + + def writeTxOnly(self, port, id, address, length, data): + result = COMM_TX_FAIL + + txpacket = [0] * (length + 12) + + txpacket[PKT_ID] = 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 + + del txpacket[:]; del txpacket + return result + + def writeTxRx(self, port, id, address, length, data, error=0): + result = COMM_TX_FAIL + + txpacket = [0] * (length + 12) + rxpacket = [] + + txpacket[PKT_ID] = 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, rxpacket, error) + + del txpacket[:]; del txpacket + return result, error + + def write1ByteTxOnly(self, port, id, address, data): + data_write = [data] + return self.writeTxOnly(port, id, address, 1, data_write) + def write1ByteTxRx(self, port, id, address, data, error=0): + data_write = [data] + return self.writeTxRx(port, id, address, 1, data_write) + + def write2ByteTxOnly(self, port, id, address, data): + data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] + return self.writeTxOnly(port, id, address, 2, data_write) + def write2ByteTxRx(self, port, id, address, data, error=0): + data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] + return self.writeTxRx(port, id, address, 2, data_write) + + def write4ByteTxOnly(self, port, 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, id, address, 4, data_write) + def write4ByteTxRx(self, port, id, address, data, error=0): + 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, id, address, 4, data_write) + + def regWriteTxOnly(self, port, id, address, length, data): + result = COMM_TX_FAIL + + txpacket = [0] * (length + 12) + + txpacket[PKT_ID] = 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 + + del txpacket[:]; del txpacket + return result + + def regWriteTxRx(self, port, id, address, length, data, error=0): + result = COMM_TX_FAIL + + txpacket = [0] * (length + 12) + rxpacket = [] + + txpacket[PKT_ID] = 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, rxpacket, error) + + del txpacket[:]; del txpacket + return result, error + + def syncReadTx(self, port, start_address, data_length, param, param_length): + result = COMM_TX_FAIL + + 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) + + del txpacket[:]; del txpacket + return result + + def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): + result = COMM_TX_FAIL + + 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) + + del txpacket[:]; del txpacket + return result + + def bulkReadTx(self, port, param, param_length): + result = COMM_TX_FAIL + + 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) + + del txpacket[:]; del txpacket + return result + + def bulkWriteTxOnly(self, port, param, param_length): + result = COMM_TX_FAIL + + 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) + + del txpacket[:]; del txpacket + return result \ No newline at end of file diff --git a/python/src/dynamixel_sdk/robotis_def.py b/python/src/dynamixel_sdk/robotis_def.py new file mode 100644 index 00000000..45ff114e --- /dev/null +++ b/python/src/dynamixel_sdk/robotis_def.py @@ -0,0 +1,68 @@ +#!/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) \ No newline at end of file diff --git a/python/tests/protocol1_0/bulk_read.py b/python/tests/protocol1_0/bulk_read.py new file mode 100644 index 00000000..d32cf025 --- /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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + + while 1: + # Bulkread present position and moving status + dxl_comm_result = groupBulkRead.txRxPacket() + if dxl_comm_result != COMM_SUCCESS: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol1_0/factory_reset.py b/python/tests/protocol1_0/factory_reset.py new file mode 100644 index 00000000..4eb5852e --- /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 packetHandler.getTxRxResult(dxl_comm_result) + quit() +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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..266e12f8 --- /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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + +# Close port1 +portHandler1.closePort() + +# Close port2 +portHandler2.closePort() \ No newline at end of file diff --git a/python/tests/protocol1_0/ping.py b/python/tests/protocol1_0/ping.py new file mode 100644 index 00000000..37c648d9 --- /dev/null +++ b/python/tests/protocol1_0/ping.py @@ -0,0 +1,100 @@ +#!/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 1.0 +# 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) +# + +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 = 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-*" + +# 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 ping the Dynamixel +# Get Dynamixel model number +dxl_model_number, dxl_comm_result, dxl_error = packetHandler.ping(portHandler, DXL_ID) +if dxl_comm_result != COMM_SUCCESS: + print packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) +else: + print "[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol1_0/read_write.py b/python/tests/protocol1_0/read_write.py new file mode 100644 index 00000000..d4d7c448 --- /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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol1_0/sync_write.py b/python/tests/protocol1_0/sync_write.py new file mode 100644 index 00000000..632f2660 --- /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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 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 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(portHandler, 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(portHandler, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) +if dxl_comm_result != COMM_SUCCESS: + print packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol2_0/broadcast_ping.py b/python/tests/protocol2_0/broadcast_ping.py new file mode 100644 index 00000000..bbd0ee07 --- /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 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() \ No newline at end of file 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..fec70803 --- /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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 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 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol2_0/factory_reset.py b/python/tests/protocol2_0/factory_reset.py new file mode 100644 index 00000000..edff990a --- /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 packetHandler.getTxRxResult(dxl_comm_result) + quit() +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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..34448c7e --- /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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 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 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol2_0/multi_port.py b/python/tests/protocol2_0/multi_port.py new file mode 100644 index 00000000..65cc28e1 --- /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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 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_PRO_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_PRO_TORQUE_ENABLE, TORQUE_DISABLE) +if dxl_comm_result != COMM_SUCCESS: + print packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 52% rename from python/protocol2_0/ping.py rename to python/tests/protocol2_0/ping.py index 333ba7a2..2a630409 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 ..src.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("Press any key to terminate...") + 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("Press any key to terminate...") + 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 packetHandler.getTxRxResult(dxl_comm_result) elif dxl_error != 0: - print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) + print packetHandler.getRxPacketError(dxl_error) else: - print("[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number)) + print "[ID:%03d] ping Succeeded. Dynamixel model number : %d" % (DXL_ID, dxl_model_number) # Close port -dynamixel.closePort(port_num) +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol2_0/read_write.py b/python/tests/protocol2_0/read_write.py new file mode 100644 index 00000000..e1b15513 --- /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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol2_0/reboot.py b/python/tests/protocol2_0/reboot.py new file mode 100644 index 00000000..8af82513 --- /dev/null +++ b/python/tests/protocol2_0/reboot.py @@ -0,0 +1,106 @@ +#!/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) + +# +# ********* reboot Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 2.0 +# 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) +# + +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 +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 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() + +# Trigger +print "Press any key to reboot" +getch() + +print "See the Dynamixel LED flickering" +# Try reboot +# Dynamixel LED will flicker while it reboots +dxl_comm_result, dxl_error = packetHandler.reboot(portHandler, DXL_ID) +if dxl_comm_result != COMM_SUCCESS: + print(packetHandler.getTxRxResult(dxl_comm_result)) +elif dxl_error != 0: + print(packetHandler.getRxPacketError(dxl_error)) + +print "[ID:%03d] reboot Succeeded\n" % DXL_ID + + +# Close port +portHandler.closePort() \ No newline at end of file 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..7be61d29 --- /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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 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 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler.getRxPacketError(dxl_error) + +# Close port +portHandler.closePort() \ No newline at end of file diff --git a/python/tests/protocol_combined.py b/python/tests/protocol_combined.py new file mode 100644 index 00000000..b3c2e4b2 --- /dev/null +++ b/python/tests/protocol_combined.py @@ -0,0 +1,199 @@ +#!/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 +import dynamixel_sdk +# from dynamixel_sdk import dynamixel_sdk # 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() + +# Open port +if portHandler.setBaudRate(BAUDRATE): + print "Succeeded to open the port" +else: + print "Failed to open the port" + 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 packetHandler1.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler2.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print 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 packetHandler1.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print packetHandler2.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 packetHandler1.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler1.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print packetHandler2.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 packetHandler1.getTxRxResult(dxl_comm_result) + elif dxl_error != 0: + print 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 packetHandler1.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler2.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 packetHandler1.getTxRxResult(dxl_comm_result) +elif dxl_error != 0: + print packetHandler2.getRxPacketError(dxl_error) + +# Close port +portHandler.closePort() From 491413f77bb91cf4f340ef316ddd669c41ef8bfb Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 26 Apr 2018 19:13:06 +0200 Subject: [PATCH 11/25] First batch of fixes --- python/setup.py | 3 +- python/src/dynamixel_sdk/packet_handler.py | 3 +- .../src/dynamixel_sdk/port_handler_linux.py | 69 +---- .../dynamixel_sdk/protocol1_packet_handler.py | 262 +++++++++--------- 4 files changed, 149 insertions(+), 188 deletions(-) diff --git a/python/setup.py b/python/setup.py index 2fd15e5e..3880e2ec 100644 --- a/python/setup.py +++ b/python/setup.py @@ -11,5 +11,6 @@ long_description=open('README.txt').read(), url='https://github.com/ROBOTIS-GIT/DynamixelSDK', author='Leon Jung', - author_email='rwjung@robotis.com' + author_email='rwjung@robotis.com', + install_requires=['serial'] ) diff --git a/python/src/dynamixel_sdk/packet_handler.py b/python/src/dynamixel_sdk/packet_handler.py index 84617d4f..50dd1036 100644 --- a/python/src/dynamixel_sdk/packet_handler.py +++ b/python/src/dynamixel_sdk/packet_handler.py @@ -23,9 +23,10 @@ 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() \ No newline at end of file + return Protocol2PacketHandler() diff --git a/python/src/dynamixel_sdk/port_handler_linux.py b/python/src/dynamixel_sdk/port_handler_linux.py index 0adee5b3..9dd0908f 100644 --- a/python/src/dynamixel_sdk/port_handler_linux.py +++ b/python/src/dynamixel_sdk/port_handler_linux.py @@ -34,7 +34,8 @@ def __init__(self, port_name): self.tx_time_per_byte = 0.0 self.is_using = False - self.setPortName(port_name) + self.port_name = port_name + self.ser = None def openPort(self): return self.setBaudRate(self.baudrate) @@ -60,7 +61,7 @@ def setBaudRate(self, baudrate): if baud <= 0: # self.setupPort(38400) # self.baudrate = baudrate - return False + return False #TODO: setCustomBaudrate(baudrate) else: self.baudrate = baudrate return self.setupPort(baud) @@ -72,28 +73,16 @@ def getBytesAvailable(self): return self.ser.in_waiting def readPort(self, length): - # read_bytes = self.ser.read(length) - read_bytes = [] - - read_bytes.extend([ord(ch) for ch in self.ser.read(length)]) - # for i in range(0, len(read_bytes)): - # read_bytes[i] = int.frombytes(read_bytes[i], byteorder = 'little') - - # for i in read_bytes: - # print i - return read_bytes + return [ord(ch) for ch in self.ser.read(length)] def writePort(self, packet): - # for i in packet: - # print i - return self.ser.write(packet) def setPacketTimeout(self, packet_length): self.packet_start_time = self.getCurrentTime() - self.packet_timeout = (self.tx_time_per_byte * self.packet_length) + (LATENCY_TIMER * 2.0) + 2.0 + self.packet_timeout = (self.tx_time_per_byte * packet_length) + (LATENCY_TIMER * 2.0) + 2.0 - def setPacketTimeout(self, msec): + def setPacketTimeoutMillis(self, msec): self.packet_start_time = self.getCurrentTime() self.packet_timeout = msec @@ -112,7 +101,7 @@ def getCurrentTime(self): def getTimeSinceStart(self): time = self.getCurrentTime() - self.packet_start_time if time < 0.0: - packet_start_time = self.getCurrentTime() + self.packet_start_time = self.getCurrentTime() return time @@ -122,8 +111,8 @@ def setupPort(self, cflag_baud): baudrate = self.baudrate, # parity = serial.PARITY_ODD, # stopbits = serial.STOPBITS_TWO, - bytesize = serial.EIGHTBITS #bytesize = serial.SEVENBITS - , timeout = 0 + bytesize = serial.EIGHTBITS, #bytesize = serial.SEVENBITS + timeout = 0 ) self.ser.reset_input_buffer() @@ -133,41 +122,7 @@ def setupPort(self, cflag_baud): return True def getCFlagBaud(self, baudrate): - if baudrate == 9600: - return 9600 - elif baudrate == 19200: - return 19200 - elif baudrate == 38400: - return 38400 - elif baudrate == 57600: - return 57600 - elif baudrate == 115200: - return 115200 - elif baudrate == 230400: - return 230400 - elif baudrate == 460800: - return 460800 - elif baudrate == 500000: - return 500000 - elif baudrate == 576000: - return 576000 - elif baudrate == 921600: - return 921600 - elif baudrate == 1000000: - return 1000000 - elif baudrate == 1152000: - return 1152000 - elif baudrate == 1500000: - return 1500000 - elif baudrate == 2000000: - return 2000000 - elif baudrate == 2500000: - return 2500000 - elif baudrate == 3000000: - return 3000000 - elif baudrate == 3500000: - return 3500000 - elif baudrate == 4000000: - return 4000000 + 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 \ No newline at end of file + return -1 diff --git a/python/src/dynamixel_sdk/protocol1_packet_handler.py b/python/src/dynamixel_sdk/protocol1_packet_handler.py index 134f082f..34aa5434 100644 --- a/python/src/dynamixel_sdk/protocol1_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -49,24 +49,32 @@ def getProtocolVersion(self): def getTxRxResult(self, result): if result == COMM_SUCCESS: return "[TxRxResult] Communication success!" - elif result == COMM_PORT_BUSY: + + if result == COMM_PORT_BUSY: return "[TxRxResult] Port is in use!" - elif result == COMM_TX_FAIL: + + if result == COMM_TX_FAIL: return "[TxRxResult] Failed transmit instruction packet!" - elif result == COMM_RX_FAIL: + + if result == COMM_RX_FAIL: return "[TxRxResult] Failed get status packet from device!" - elif result == COMM_TX_ERROR: + + if result == COMM_TX_ERROR: return "[TxRxResult] Incorrect instruction packet!" - elif result == COMM_RX_WAITING: + + if result == COMM_RX_WAITING: return "[TxRxResult] Now receiving status packet!" - elif result == COMM_RX_TIMEOUT: + + if result == COMM_RX_TIMEOUT: return "[TxRxResult] There is no status packet!" - elif result == COMM_RX_CORRUPT: + + if result == COMM_RX_CORRUPT: return "[TxRxResult] Incorrect status packet!" - elif result == COMM_NOT_AVAILABLE: + + if result == COMM_NOT_AVAILABLE: return "[TxRxResult] Protocol does not support this function!" - else: - return "" + + return "" def getRxPacketError(self, error): if error & ERRBIT_VOLTAGE: @@ -131,7 +139,7 @@ def rxPacket(self, port, rxpacket=[]): wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) while True: - rxpacket += port.readPort(wait_length - rx_length) + rxpacket.append(port.readPort(wait_length - rx_length)) rx_length = len(rxpacket) if rx_length >= wait_length: # find packet header @@ -154,7 +162,7 @@ def rxPacket(self, port, rxpacket=[]): if rx_length < wait_length: # check timeout - if port.isPacketTimeout() == True: + if port.isPacketTimeout(): if rx_length == 0: result = COMM_RX_TIMEOUT print "COMM_RX_TIMEOUT 1" @@ -188,7 +196,7 @@ def rxPacket(self, port, rxpacket=[]): else: # check timeout - if port.isPacketTimeout() == True: + if port.isPacketTimeout(): if rx_length == 0: result = COMM_RX_TIMEOUT print "COMM_RX_TIMEOUT 3" @@ -202,8 +210,9 @@ def rxPacket(self, port, rxpacket=[]): return rxpacket, result # NOT for BulkRead - def txRxPacket(self, port, txpacket, rxpacket=[], error=0): + def txRxPacket(self, port, txpacket, rxpacket=[]): result = COMM_TX_FAIL + error = 0 # tx packet result = self.txPacket(port, txpacket) @@ -228,34 +237,36 @@ def txRxPacket(self, port, txpacket, rxpacket=[], error=0): # rx packet while True: - rxpacket, result = self.rxPacket(port, rxpacket) - + _rxpacket, result = self.rxPacket(port, rxpacket) + del rxpacket[:] + rxpacket.extend(_rxpacket[:]) if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: break if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: - if error != 0: - error = rxpacket[PKT_ERROR] + error = rxpacket[PKT_ERROR] return rxpacket, result, error - def ping(self, port, id, model_number=0, error=0): + def ping(self, port, dxl_id): result = COMM_TX_FAIL + model_number=0 + error=0 txpacket = [0] * 6 rxpacket = [] - if id >= BROADCAST_ID: + if dxl_id >= BROADCAST_ID: return model_number, COMM_NOT_AVAILABLE, error - txpacket[PKT_ID] = id + txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH] = 2 txpacket[PKT_INSTRUCTION] = INST_PING - rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket, error) + rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket) if result == COMM_SUCCESS: - data_read, result, error = self.readTxRx(port, id, 0, 2) # Address 0 : Model Number + 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]) @@ -264,10 +275,10 @@ def ping(self, port, id, model_number=0, error=0): def broadcastPing(self, port, data_list={}): return data_list, COMM_NOT_AVAILABLE - def action(self, port, id): + def action(self, port, dxl_id): txpacket = [0] * 6 - txpacket[PKT_ID] = id + txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH] = 2 txpacket[PKT_INSTRUCTION] = INST_ACTION @@ -275,29 +286,29 @@ def action(self, port, id): return result - def reboot(self, port, id, error=0): - return COMM_NOT_AVAILABLE, error + def reboot(self, port, dxl_id): + return COMM_NOT_AVAILABLE, 0 - def factoryReset(self, port, id, option, error=0): + def factoryReset(self, port, dxl_id): txpacket = [0] * 6 rxpacket = [] - txpacket[PKT_ID] = id + txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH] = 2 txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET - _, result, error = self.txRxPacket(port, txpacket, rxpacket, error) + _, result, error = self.txRxPacket(port, txpacket, rxpacket) return result, error - def readTx(self, port, id, address, length): + def readTx(self, port, dxl_id, address, length): result = COMM_TX_FAIL txpacket = [0] * 8 - if id >= BROADCAST_ID: + if dxl_id >= BROADCAST_ID: return COMM_NOT_AVAILABLE - txpacket[PKT_ID] = id + txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH] = 4 txpacket[PKT_INSTRUCTION] = INST_READ txpacket[PKT_PARAMETER0+0] = address @@ -306,112 +317,104 @@ def readTx(self, port, id, address, length): result = self.txPacket(port, txpacket) # set packet timeout - if (result == COMM_SUCCESS): + if result == COMM_SUCCESS: port.setPacketTimeout(length + 6) return result - def readRx(self, port, id, length, data=[], error=0): + def readRx(self, port, dxl_id, length, data=[]): result = COMM_TX_FAIL - + error=0 rxpacket = [] - if len(data) != 0: - data = [] + del data[:] while True: rxpacket, result = self.rxPacket(port, rxpacket) - if result != COMM_SUCCESS or rxpacket[PKT_ID] == id: + if result != COMM_SUCCESS or rxpacket[PKT_ID] == dxl_id: break - if result == COMM_SUCCESS and rxpacket[PKT_ID] == id: + if result == COMM_SUCCESS and rxpacket[PKT_ID] == dxl_id: if error != 0: error = rxpacket[PKT_ERROR] data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) - del rxpacket[:]; del rxpacket return data, result, error - def readTxRx(self, port, id, address, length, data=[], error=0): + def readTxRx(self, port, dxl_id, address, length, data=[]): result = COMM_TX_FAIL + error = 0 txpacket = [0] * 8 - rxpacket = [] - if len(data) != 0: - data = [] + del data[:] - if id >= BROADCAST_ID: + if dxl_id >= BROADCAST_ID: return data, COMM_NOT_AVAILABLE, error - txpacket[PKT_ID] = id + 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, rxpacket, error) + rxpacket, result, error = self.txRxPacket(port, txpacket, []) if result == COMM_SUCCESS: if error != 0: error = rxpacket[PKT_ERROR] data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) - del rxpacket[:]; del rxpacket - return data, result, error - - def read1ByteTx(self, port, id, address): - return self.readTx(port, id, address, 1) - def read1ByteRx(self, port, id, data=[], error=0): - data_read = [0] - data_read, result, error = self.readRx(port, id, 1, data_read, error) - if result == COMM_SUCCESS: - data = data_read[0] - return data, result, error - def read1ByteTxRx(self, port, id, address, data=[], error=0): - data_read = [0] - data_read, result, error = self.readTxRx(port, id, address, 1, data_read, error) - if result == COMM_SUCCESS: - data = data_read[0] return data, result, error - def read2ByteTx(self, port, id, address): - return self.readTx(port, id, address, 2) - def read2ByteRx(self, port, id, data=[], error=0): - data_read = [0] * 2 - data_read, result, error = self.readRx(port, id, 2, data_read, error) - if result == COMM_SUCCESS: - data = DXL_MAKEWORD(data_read[0], data_read[1]) - return data, result, error - def read2ByteTxRx(self, port, id, address, data=[], error=0): - data_read = [0] * 2 - data_read, result, error = self.readTxRx(port, id, address, 2, data_read, error) - if result == COMM_SUCCESS: - data = DXL_MAKEWORD(data_read[0], data_read[1]) - return data, result, error - - def read4ByteTx(self, port, id, address): - return self.readTx(port, id, address, 4) - def read4ByteRx(self, port, id, data=[], error=0): - data_read = [0] * 4 - data_read, result, error = self.readRx(port, id, 4, data_read, error) - if result == COMM_SUCCESS: - data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) - return data, result, error - def read4ByteTxRx(self, port, id, address, data=[], error=0): - data_read = [0] * 4 - data_read, result, error = self.readTxRx(port, id, address, 4, data_read, error) - if result == COMM_SUCCESS: - data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) - 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=[]): + data, result, error = self.readRx(port, dxl_id, 1, data) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error + + def read1ByteTxRx(self, port, dxl_id, address, data=[]): + data, result, error = self.readTxRx(port, dxl_id, address, 1, data) + data_read = data[0] if (result == COMM_SUCCESS) else 0 + return data_read, result, error - def writeTxOnly(self, port, id, address, length, data): + def read2ByteTx(self, port, dxl_id, address): + return self.readTx(port, dxl_id, address, 2) + + def read2ByteRx(self, port, dxl_id, data=[]): + data, result, error = self.readRx(port, dxl_id, 2, data) + 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=[]): + data, result, error = self.readTxRx(port, dxl_id, address, 2, data) + 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=[]): + data, result, error = self.readRx(port, dxl_id, 4, data) + 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=[]): + data, result, error = self.readTxRx(port, dxl_id, address, 4, data) + 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): result = COMM_TX_FAIL txpacket = [0] * (length + 7) - txpacket[PKT_ID] = id + txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH] = length + 3 txpacket[PKT_INSTRUCTION] = INST_WRITE txpacket[PKT_PARAMETER0] = address @@ -422,82 +425,85 @@ def writeTxOnly(self, port, id, address, length, data): result = self.txPacket(port, txpacket) port.is_using = False - del txpacket[:]; del txpacket return result - def writeTxRx(self, port, id, address, length, data, error=0): + def writeTxRx(self, port, dxl_id, address, length, data): result = COMM_TX_FAIL txpacket = [0] * (length + 7) rxpacket = [] - txpacket[PKT_ID] = id + 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, rxpacket, error) + rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket) - del txpacket[:]; del txpacket return result, error - def write1ByteTxOnly(self, port, id, address, data): + def write1ByteTxOnly(self, port, dxl_id, address, data): data_write = [data] - return self.writeTxOnly(port, id, address, 1, data_write) - def write1ByteTxRx(self, port, id, address, data, error=0): + 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, id, address, 1, data_write) + return self.writeTxRx(port, dxl_id, address, 1, data_write) - def write2ByteTxOnly(self, port, id, address, data): - data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] - return self.writeTxOnly(port, id, address, 2, data_write) - def write2ByteTxRx(self, port, id, address, data, error=0): + def write2ByteTxOnly(self, port, dxl_id, address, data): data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] - return self.writeTxRx(port, id, address, 2, data_write) - - def write4ByteTxOnly(self, port, 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, id, address, 4, data_write) - def write4ByteTxRx(self, port, id, address, data, error=0): - 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, id, address, 4, data_write) - + return self.writeTxOnly(port, dxl_id, address, 2, data_write) - def regWriteTxOnly(self, port, id, address, length, data): + 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): result = COMM_TX_FAIL txpacket = [0] * (length + 6) - txpacket[PKT_ID] = id + 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 - del txpacket[:]; del txpacket return result - def regWriteTxRx(self, port, id, address, length, data, error=0): + def regWriteTxRx(self, port, dxl_id, address, length, data): result = COMM_TX_FAIL txpacket = [0] * (length + 6) - rxpacket = [] - txpacket[PKT_ID] = id + 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, rxpacket, error) + _, result, error = self.txRxPacket(port, txpacket, []) - del txpacket[:]; del txpacket return result, error def syncReadTx(self, port, start_address, data_length, param, param_length): @@ -519,7 +525,6 @@ def syncWriteTxOnly(self, port, start_address, data_length, param, param_length) result = self.txRxPacket(port, txpacket) - del txpacket[:]; del txpacket return result def bulkReadTx(self, port, param, param_length): @@ -544,8 +549,7 @@ def bulkReadTx(self, port, param, param_length): i += 3 port.setPacketTimeout(wait_length) - del txpacket[:]; del txpacket return result def bulkWriteTxOnly(self, port, param, param_length): - return COMM_NOT_AVAILABLE \ No newline at end of file + return COMM_NOT_AVAILABLE From 523250968209413028f0ff0bf9feb608bf044e31 Mon Sep 17 00:00:00 2001 From: leon Date: Mon, 30 Apr 2018 10:43:22 +0900 Subject: [PATCH 12/25] fixed: SDK python setup.py serial to pyserial --- python/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/setup.py b/python/setup.py index 3880e2ec..4985f3d2 100644 --- a/python/setup.py +++ b/python/setup.py @@ -12,5 +12,5 @@ url='https://github.com/ROBOTIS-GIT/DynamixelSDK', author='Leon Jung', author_email='rwjung@robotis.com', - install_requires=['serial'] + install_requires=['pyserial'] ) From fd01d54945f4611bfd2c19f20b77aecc2f8145fe Mon Sep 17 00:00:00 2001 From: leon Date: Mon, 30 Apr 2018 14:39:26 +0900 Subject: [PATCH 13/25] some bugs in p1ph fixed... now for removing rxpacket inpass parameter --- .../dynamixel_sdk/protocol1_packet_handler.py | 42 +++++++++---------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/python/src/dynamixel_sdk/protocol1_packet_handler.py b/python/src/dynamixel_sdk/protocol1_packet_handler.py index ce97e314..1f79b2dc 100644 --- a/python/src/dynamixel_sdk/protocol1_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -49,32 +49,24 @@ def getProtocolVersion(self): def getTxRxResult(self, result): if result == COMM_SUCCESS: return "[TxRxResult] Communication success!" - - if result == COMM_PORT_BUSY: + elif result == COMM_PORT_BUSY: return "[TxRxResult] Port is in use!" - - if result == COMM_TX_FAIL: + elif result == COMM_TX_FAIL: return "[TxRxResult] Failed transmit instruction packet!" - - if result == COMM_RX_FAIL: + elif result == COMM_RX_FAIL: return "[TxRxResult] Failed get status packet from device!" - - if result == COMM_TX_ERROR: + elif result == COMM_TX_ERROR: return "[TxRxResult] Incorrect instruction packet!" - - if result == COMM_RX_WAITING: + elif result == COMM_RX_WAITING: return "[TxRxResult] Now receiving status packet!" - - if result == COMM_RX_TIMEOUT: + elif result == COMM_RX_TIMEOUT: return "[TxRxResult] There is no status packet!" - - if result == COMM_RX_CORRUPT: + elif result == COMM_RX_CORRUPT: return "[TxRxResult] Incorrect status packet!" - - if result == COMM_NOT_AVAILABLE: + elif result == COMM_NOT_AVAILABLE: return "[TxRxResult] Protocol does not support this function!" - - return "" + else: + return "" def getRxPacketError(self, error): if error & ERRBIT_VOLTAGE: @@ -139,7 +131,7 @@ def rxPacket(self, port, rxpacket=[]): wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) while True: - rxpacket.append(port.readPort(wait_length - rx_length)) + rxpacket.extend(port.readPort(wait_length - rx_length)) rx_length = len(rxpacket) if rx_length >= wait_length: # find packet header @@ -229,9 +221,13 @@ def txRxPacket(self, port, txpacket, rxpacket=[]): # rx packet while True: - _rxpacket, result = self.rxPacket(port, rxpacket) - del rxpacket[:] - rxpacket.extend(_rxpacket[:]) + rxpacket, result = self.rxPacket(port, rxpacket) + # _rxpacket, result = self.rxPacket(port, rxpacket) + # del rxpacket[:] + # print rxpacket + # print _rxpacket + # print rxpacket + # rxpacket.extend(_rxpacket[:]) if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: break @@ -240,7 +236,7 @@ def txRxPacket(self, port, txpacket, rxpacket=[]): return rxpacket, result, error - def ping(self, port, dxl_id): + def ping(self, port, dxl_id): result = COMM_TX_FAIL model_number=0 error=0 From b19434d49b353d00b99b82c1b52d52c99fa36fab Mon Sep 17 00:00:00 2001 From: leon Date: Mon, 30 Apr 2018 15:12:33 +0900 Subject: [PATCH 14/25] rxpacket param input removed --- python/src/dynamixel_sdk/group_bulk_read.py | 2 +- python/src/dynamixel_sdk/group_sync_read.py | 2 +- .../dynamixel_sdk/protocol1_packet_handler.py | 86 +++++++++---------- 3 files changed, 43 insertions(+), 47 deletions(-) diff --git a/python/src/dynamixel_sdk/group_bulk_read.py b/python/src/dynamixel_sdk/group_bulk_read.py index cc2082b6..79cb0f1a 100644 --- a/python/src/dynamixel_sdk/group_bulk_read.py +++ b/python/src/dynamixel_sdk/group_bulk_read.py @@ -98,7 +98,7 @@ def rxPacket(self): return COMM_NOT_AVAILABLE for id in self.data_dict: - self.data_dict[id][PARAM_NUM_DATA], result, _ = self.ph.readRx(self.port, id, self.data_dict[id][PARAM_NUM_LENGTH], self.data_dict[id][PARAM_NUM_DATA]) + self.data_dict[id][PARAM_NUM_DATA], result, _ = self.ph.readRx(self.port, id, self.data_dict[id][PARAM_NUM_LENGTH]) if result != COMM_SUCCESS: return result diff --git a/python/src/dynamixel_sdk/group_sync_read.py b/python/src/dynamixel_sdk/group_sync_read.py index 9438dc12..c58dbbb7 100644 --- a/python/src/dynamixel_sdk/group_sync_read.py +++ b/python/src/dynamixel_sdk/group_sync_read.py @@ -97,7 +97,7 @@ def rxPacket(self): return COMM_NOT_AVAILABLE for id in self.data_dict: - self.data_dict[id], result, _ = self.ph.readRx(self.port, id, self.data_length, self.data_dict[id]) + self.data_dict[id], result, _ = self.ph.readRx(self.port, id, self.data_length) if result != COMM_SUCCESS: return result diff --git a/python/src/dynamixel_sdk/protocol1_packet_handler.py b/python/src/dynamixel_sdk/protocol1_packet_handler.py index 1f79b2dc..faeef6ac 100644 --- a/python/src/dynamixel_sdk/protocol1_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -124,7 +124,9 @@ def txPacket(self, port, txpacket): return COMM_SUCCESS - def rxPacket(self, port, rxpacket=[]): + def rxPacket(self, port): + rxpacket = [] + result = COMM_TX_FAIL checksum = 0 rx_length = 0 @@ -194,7 +196,8 @@ def rxPacket(self, port, rxpacket=[]): return rxpacket, result # NOT for BulkRead - def txRxPacket(self, port, txpacket, rxpacket=[]): + def txRxPacket(self, port, txpacket): + rxpacket = None result = COMM_TX_FAIL error = 0 @@ -221,13 +224,7 @@ def txRxPacket(self, port, txpacket, rxpacket=[]): # rx packet while True: - rxpacket, result = self.rxPacket(port, rxpacket) - # _rxpacket, result = self.rxPacket(port, rxpacket) - # del rxpacket[:] - # print rxpacket - # print _rxpacket - # print rxpacket - # rxpacket.extend(_rxpacket[:]) + rxpacket, result = self.rxPacket(port) if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: break @@ -238,11 +235,11 @@ def txRxPacket(self, port, txpacket, rxpacket=[]): def ping(self, port, dxl_id): result = COMM_TX_FAIL - model_number=0 - error=0 + model_number = 0 + error = 0 txpacket = [0] * 6 - rxpacket = [] + rxpacket = None if dxl_id >= BROADCAST_ID: return model_number, COMM_NOT_AVAILABLE, error @@ -251,7 +248,7 @@ def ping(self, port, dxl_id): txpacket[PKT_LENGTH] = 2 txpacket[PKT_INSTRUCTION] = INST_PING - rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket) + 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 @@ -260,7 +257,8 @@ def ping(self, port, dxl_id): return model_number, result, error - def broadcastPing(self, port, data_list={}): + def broadcastPing(self, port): + data_list = None return data_list, COMM_NOT_AVAILABLE def action(self, port, dxl_id): @@ -279,13 +277,12 @@ def reboot(self, port, dxl_id): def factoryReset(self, port, dxl_id): txpacket = [0] * 6 - rxpacket = [] txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH] = 2 txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET - _, result, error = self.txRxPacket(port, txpacket, rxpacket) + _, result, error = self.txRxPacket(port, txpacket) return result, error def readTx(self, port, dxl_id, address, length): @@ -310,33 +307,33 @@ def readTx(self, port, dxl_id, address, length): return result - def readRx(self, port, dxl_id, length, data=[]): + def readRx(self, port, dxl_id, length): result = COMM_TX_FAIL - error=0 - rxpacket = [] + error = 0 + rxpacket = None - del data[:] + data = [] while True: - rxpacket, result = self.rxPacket(port, rxpacket) + 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: - if error != 0: - error = rxpacket[PKT_ERROR] + error = rxpacket[PKT_ERROR] data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) return data, result, error - def readTxRx(self, port, dxl_id, address, length, data=[]): + def readTxRx(self, port, dxl_id, address, length): result = COMM_TX_FAIL error = 0 txpacket = [0] * 8 + rxpacket = None - del data[:] + data = [] if dxl_id >= BROADCAST_ID: return data, COMM_NOT_AVAILABLE, error @@ -347,10 +344,9 @@ def readTxRx(self, port, dxl_id, address, length, data=[]): txpacket[PKT_PARAMETER0+0] = address txpacket[PKT_PARAMETER0+1] = length - rxpacket, result, error = self.txRxPacket(port, txpacket, []) + rxpacket, result, error = self.txRxPacket(port, txpacket) if result == COMM_SUCCESS: - if error != 0: - error = rxpacket[PKT_ERROR] + error = rxpacket[PKT_ERROR] data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) @@ -359,40 +355,40 @@ def readTxRx(self, port, dxl_id, address, length, data=[]): def read1ByteTx(self, port, dxl_id, address): return self.readTx(port, dxl_id, address, 1) - def read1ByteRx(self, port, dxl_id, data=[]): - data, result, error = self.readRx(port, dxl_id, 1, data) + 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=[]): - data, result, error = self.readTxRx(port, dxl_id, address, 1, data) + 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=[]): - data, result, error = self.readRx(port, dxl_id, 2, data) + 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=[]): - data, result, error = self.readTxRx(port, dxl_id, address, 2, data) + 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=[]): - data, result, error = self.readRx(port, dxl_id, 4, data) + 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=[]): - data, result, error = self.readTxRx(port, dxl_id, address, 4, data) + 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 @@ -416,18 +412,18 @@ def writeTxOnly(self, port, dxl_id, address, length, data): return result def writeTxRx(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL txpacket = [0] * (length + 7) - rxpacket = [] + rxpacket = None + 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, rxpacket) + rxpacket, result, error = self.txRxPacket(port, txpacket) return result, error @@ -490,7 +486,7 @@ def regWriteTxRx(self, port, dxl_id, address, length, data): txpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length] = data[0 : length] - _, result, error = self.txRxPacket(port, txpacket, []) + _, result, error = self.txRxPacket(port, txpacket) return result, error @@ -511,7 +507,7 @@ def syncWriteTxOnly(self, port, start_address, data_length, param, param_length) txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + param_length] = param[0 : param_length] - result = self.txRxPacket(port, txpacket) + _, result, _ = self.txRxPacket(port, txpacket) return result From dd0da124f08fe7e3ebb28c043dc5361043408b16 Mon Sep 17 00:00:00 2001 From: leon Date: Mon, 30 Apr 2018 15:37:55 +0900 Subject: [PATCH 15/25] space in each line removed --- .../dynamixel_sdk/protocol1_packet_handler.py | 167 +++++++++--------- 1 file changed, 83 insertions(+), 84 deletions(-) diff --git a/python/src/dynamixel_sdk/protocol1_packet_handler.py b/python/src/dynamixel_sdk/protocol1_packet_handler.py index faeef6ac..5d4233a7 100644 --- a/python/src/dynamixel_sdk/protocol1_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -21,26 +21,26 @@ from .robotis_def import * -TXPACKET_MAX_LEN = 250 -RXPACKET_MAX_LEN = 250 +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 +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. +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): @@ -127,10 +127,10 @@ def txPacket(self, port, txpacket): 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) + 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)) @@ -262,11 +262,11 @@ def broadcastPing(self, port): return data_list, COMM_NOT_AVAILABLE def action(self, port, dxl_id): - txpacket = [0] * 6 + txpacket = [0] * 6 - txpacket[PKT_ID] = dxl_id - txpacket[PKT_LENGTH] = 2 - txpacket[PKT_INSTRUCTION] = INST_ACTION + txpacket[PKT_ID] = dxl_id + txpacket[PKT_LENGTH] = 2 + txpacket[PKT_INSTRUCTION] = INST_ACTION _, result, _ = self.txRxPacket(port, txpacket) @@ -276,28 +276,28 @@ def reboot(self, port, dxl_id): return COMM_NOT_AVAILABLE, 0 def factoryReset(self, port, dxl_id): - txpacket = [0] * 6 + txpacket = [0] * 6 - txpacket[PKT_ID] = dxl_id - txpacket[PKT_LENGTH] = 2 - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET + 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): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * 8 + 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 + 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) @@ -308,10 +308,10 @@ def readTx(self, port, dxl_id, address, length): return result def readRx(self, port, dxl_id, length): - result = COMM_TX_FAIL + result = COMM_TX_FAIL error = 0 - rxpacket = None + rxpacket = None data = [] while True: @@ -328,21 +328,21 @@ def readRx(self, port, dxl_id, length): return data, result, error def readTxRx(self, port, dxl_id, address, length): - result = COMM_TX_FAIL + result = COMM_TX_FAIL error = 0 - txpacket = [0] * 8 - rxpacket = None + txpacket = [0] * 8 + rxpacket = None data = [] if dxl_id >= BROADCAST_ID: return data, COMM_NOT_AVAILABLE, error - txpacket[PKT_ID] = dxl_id - txpacket[PKT_LENGTH] = 4 - txpacket[PKT_INSTRUCTION] = INST_READ - txpacket[PKT_PARAMETER0+0] = address - txpacket[PKT_PARAMETER0+1] = length + 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: @@ -394,15 +394,14 @@ def read4ByteTxRx(self, port, dxl_id, address): return data_read, result, error def writeTxOnly(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL - - txpacket = [0] * (length + 7) + result = COMM_TX_FAIL - txpacket[PKT_ID] = dxl_id - txpacket[PKT_LENGTH] = length + 3 - txpacket[PKT_INSTRUCTION] = INST_WRITE - txpacket[PKT_PARAMETER0] = address + 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] @@ -412,15 +411,15 @@ def writeTxOnly(self, port, dxl_id, address, length, data): return result def writeTxRx(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * (length + 7) - rxpacket = None + txpacket = [0] * (length + 7) + rxpacket = None - txpacket[PKT_ID] = dxl_id - txpacket[PKT_LENGTH] = length + 3 - txpacket[PKT_INSTRUCTION] = INST_WRITE - txpacket[PKT_PARAMETER0] = address + 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) @@ -458,14 +457,14 @@ def write4ByteTxRx(self, port, dxl_id, address, data): return self.writeTxRx(port, dxl_id, address, 4, data_write) def regWriteTxOnly(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * (length + 6) + txpacket = [0] * (length + 6) - txpacket[PKT_ID] = dxl_id - txpacket[PKT_LENGTH] = length + 3 - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE - txpacket[PKT_PARAMETER0] = address + 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] @@ -475,14 +474,14 @@ def regWriteTxOnly(self, port, dxl_id, address, length, data): return result def regWriteTxRx(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * (length + 6) + txpacket = [0] * (length + 6) - txpacket[PKT_ID] = dxl_id - txpacket[PKT_LENGTH] = length + 3 - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE - txpacket[PKT_PARAMETER0] = address + 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] @@ -494,16 +493,16 @@ 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): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * (param_length + 8) + 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_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] @@ -512,15 +511,15 @@ def syncWriteTxOnly(self, port, start_address, data_length, param, param_length) return result def bulkReadTx(self, port, param, param_length): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * (param_length + 7) + 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_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] From 14d344c6fccf52b92605f2d567485a1b997bba51 Mon Sep 17 00:00:00 2001 From: leon Date: Mon, 30 Apr 2018 16:34:46 +0900 Subject: [PATCH 16/25] keep mofifying as follow the PEP8 --- python/src/dynamixel_sdk/group_bulk_read.py | 6 +- .../dynamixel_sdk/protocol1_packet_handler.py | 19 +- .../dynamixel_sdk/protocol2_packet_handler.py | 499 +++++++++--------- python/src/dynamixel_sdk/robotis_def.py | 46 +- 4 files changed, 278 insertions(+), 292 deletions(-) diff --git a/python/src/dynamixel_sdk/group_bulk_read.py b/python/src/dynamixel_sdk/group_bulk_read.py index 79cb0f1a..5e91306c 100644 --- a/python/src/dynamixel_sdk/group_bulk_read.py +++ b/python/src/dynamixel_sdk/group_bulk_read.py @@ -21,9 +21,9 @@ from .robotis_def import * -PARAM_NUM_DATA = 0 -PARAM_NUM_ADDRESS = 1 -PARAM_NUM_LENGTH = 2 +PARAM_NUM_DATA = 0 +PARAM_NUM_ADDRESS = 1 +PARAM_NUM_LENGTH = 2 class GroupBulkRead: def __init__(self, port, ph): diff --git a/python/src/dynamixel_sdk/protocol1_packet_handler.py b/python/src/dynamixel_sdk/protocol1_packet_handler.py index 5d4233a7..164e4bb4 100644 --- a/python/src/dynamixel_sdk/protocol1_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -255,6 +255,7 @@ def ping(self, port, dxl_id): if result == COMM_SUCCESS: model_number = DXL_MAKEWORD(data_read[0], data_read[1]) + del rxpacket[:]; del rxpacket return model_number, result, error def broadcastPing(self, port): @@ -283,6 +284,7 @@ def factoryReset(self, port, dxl_id): txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET _, result, error = self.txRxPacket(port, txpacket) + return result, error def readTx(self, port, dxl_id, address, length): @@ -325,6 +327,7 @@ def readRx(self, port, dxl_id, length): data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) + del rxpacket[:]; del rxpacket return data, result, error def readTxRx(self, port, dxl_id, address, length): @@ -350,16 +353,15 @@ def readTxRx(self, port, dxl_id, address, length): data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) + del rxpacket[:]; del rxpacket return data, result, error def read1ByteTx(self, port, dxl_id, address): - return self.readTx(port, dxl_id, address, 1) - + 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 - + 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 @@ -367,12 +369,10 @@ def read1ByteTxRx(self, port, dxl_id, address): 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 @@ -380,13 +380,11 @@ def read2ByteTxRx(self, port, dxl_id, address): 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]), @@ -424,12 +422,12 @@ def writeTxRx(self, port, dxl_id, address, length, data): txpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length] = data[0 : length] rxpacket, result, error = self.txRxPacket(port, txpacket) + del rxpacket[:]; del rxpacket 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) @@ -437,7 +435,6 @@ def write1ByteTxRx(self, port, dxl_id, address, data): 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) @@ -448,7 +445,6 @@ def write4ByteTxOnly(self, port, dxl_id, address, 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)), @@ -475,6 +471,7 @@ def regWriteTxOnly(self, port, dxl_id, address, length, data): def regWriteTxRx(self, port, dxl_id, address, length, data): result = COMM_TX_FAIL + error = 0 txpacket = [0] * (length + 6) diff --git a/python/src/dynamixel_sdk/protocol2_packet_handler.py b/python/src/dynamixel_sdk/protocol2_packet_handler.py index 12501706..6e7769c4 100644 --- a/python/src/dynamixel_sdk/protocol2_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol2_packet_handler.py @@ -21,31 +21,31 @@ from .robotis_def import * -TXPACKET_MAX_LEN = 4 * 1024 -RXPACKET_MAX_LEN = 4 * 1024 +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 +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 +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. +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): @@ -78,7 +78,6 @@ def getRxPacketError(self, error): 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: @@ -152,10 +151,10 @@ def updateCRC(self, crc_accum, data_blk_ptr, data_blk_size): 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 + packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]) + packet_length_out = packet_length_in - temp = [0] * TXPACKET_MAX_LEN + temp = [0] * TXPACKET_MAX_LEN temp[PKT_HEADER0 : PKT_HEADER0 + PKT_LENGTH_H + 1] = packet[PKT_HEADER0 : PKT_HEADER0 + PKT_LENGTH_H + 1] # FF FF FD XX ID LEN_L LEN_H @@ -210,7 +209,7 @@ def txPacket(self, port, txpacket): if port.is_using: return COMM_PORT_BUSY port.is_using = True - + # byte stuffing for header self.addStuffing(txpacket) @@ -243,14 +242,15 @@ def txPacket(self, port, txpacket): return COMM_SUCCESS - def rxPacket(self, port, rxpacket=[]): - result = COMM_TX_FAIL + 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 += port.readPort(wait_length - rx_length) + rxpacket.extend(port.readPort(wait_length - rx_length)) rx_length = len(rxpacket) if rx_length >= wait_length: # find packet header @@ -290,8 +290,7 @@ def rxPacket(self, port, rxpacket=[]): else: # remove unnecessary packets del rxpacket[0 : idx] - - rx_length = rx_length - idx + rx_length -= idx else: if port.isPacketTimeout() == True: @@ -309,8 +308,10 @@ def rxPacket(self, port, rxpacket=[]): return rxpacket, result # NOT for BulkRead / SyncRead instruction - def txRxPacket(self, port, txpacket, rxpacket=[], error=0): + def txRxPacket(self, port, txpacket): + rxpacket = None result = COMM_TX_FAIL + error = 0 # tx packet result = self.txPacket(port, txpacket) @@ -336,22 +337,22 @@ def txRxPacket(self, port, txpacket, rxpacket=[], error=0): # rx packet while True: - rxpacket, result = self.rxPacket(port, rxpacket) - + 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]: - if error != 0: - error = rxpacket[PKT_ERROR] + error = rxpacket[PKT_ERROR] return rxpacket, result, error - def ping(self, port, id, model_number=0, error=0): + def ping(self, port, id): result = COMM_TX_FAIL + model_number = 0 + error = 0 txpacket = [0] * 10 - rxpacket = [] + rxpacket = None if id >= BROADCAST_ID: return model_number, COMM_NOT_AVAILABLE, error @@ -361,26 +362,29 @@ def ping(self, port, id, model_number=0, error=0): txpacket[PKT_LENGTH_H] = 0 txpacket[PKT_INSTRUCTION] = INST_PING - rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket, error) + rxpacket, result, error = self.txRxPacket(port, txpacket) if result == COMM_SUCCESS: model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]) + del rxpacket[:]; del rxpacket return model_number, result, error - def broadcastPing(self, port, data_list={}): - STATUS_LENGTH = 14 - result = COMM_TX_FAIL + def broadcastPing(self, port): + data_list = {} + + STATUS_LENGTH = 14 + result = COMM_TX_FAIL - rx_length = 0 - wait_length = STATUS_LENGTH * MAX_ID + rx_length = 0 + wait_length = STATUS_LENGTH * MAX_ID - txpacket = [0] * 10 - rxpacket = [] + txpacket = [0] * 10 + rxpacket = [] - txpacket[PKT_ID] = BROADCAST_ID - txpacket[PKT_LENGTH_L] = 3 - txpacket[PKT_LENGTH_H] = 0 - txpacket[PKT_INSTRUCTION] = INST_PING + 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: @@ -408,7 +412,7 @@ def broadcastPing(self, port, data_list={}): # 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: + 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 @@ -438,61 +442,63 @@ def broadcastPing(self, port, data_list={}): del rxpacket[0 : idx] rx_length = rx_length - idx + del rxpacket[:]; del rxpacket return data_list, result def action(self, port, id): - txpacket = [0] * 10 + txpacket = [0] * 10 - txpacket[PKT_ID] = id - txpacket[PKT_LENGTH_L] = 3 - txpacket[PKT_LENGTH_H] = 0 - txpacket[PKT_INSTRUCTION] = INST_ACTION + txpacket[PKT_ID] = 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, id, error=0): - txpacket = [0] * 10 - rxpacket = [] + def reboot(self, port, id): + error = 0 + + txpacket = [0] * 10 - txpacket[PKT_ID] = id - txpacket[PKT_LENGTH_L] = 3 - txpacket[PKT_LENGTH_H] = 0 - txpacket[PKT_INSTRUCTION] = INST_REBOOT + txpacket[PKT_ID] = id + txpacket[PKT_LENGTH_L] = 3 + txpacket[PKT_LENGTH_H] = 0 + txpacket[PKT_INSTRUCTION] = INST_REBOOT - rxpacket, result, error = self.txRxPacket(port, txpacket, rxpacket, error) + _, result, error = self.txRxPacket(port, txpacket) return result, error - def factoryReset(self, port, id, option, error=0): - txpacket = [0] * 11 - rxpacket = [] + def factoryReset(self, port, id, option): + error = 0 - txpacket[PKT_ID] = id - txpacket[PKT_LENGTH_L] = 4 - txpacket[PKT_LENGTH_H] = 0 - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET - txpacket[PKT_PARAMETER0] = option + txpacket = [0] * 11 - _, result, error = self.txRxPacket(port, txpacket, rxpacket, error) + txpacket[PKT_ID] = 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, id, address, length): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * 14 + txpacket = [0] * 14 if id >= BROADCAST_ID: return COMM_NOT_AVAILABLE - txpacket[PKT_ID] = 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) + txpacket[PKT_ID] = 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) @@ -502,221 +508,208 @@ def readTx(self, port, id, address, length): return result - def readRx(self, port, id, length, data=[], error=0): - result = COMM_TX_FAIL - - rxpacket = [] + def readRx(self, port, id, length): + result = COMM_TX_FAIL + error = 0 - if len(data) != 0: - data = [] + rxpacket = None + data = [] while True: - rxpacket, result = self.rxPacket(port, rxpacket) + rxpacket, result = self.rxPacket(port) if result != COMM_SUCCESS or rxpacket[PKT_ID] == id: break if result == COMM_SUCCESS and rxpacket[PKT_ID] == id: - if error != 0: - error = rxpacket[PKT_ERROR] + error = rxpacket[PKT_ERROR] data.extend(rxpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length]) del rxpacket[:]; del rxpacket return data, result, error - def readTxRx(self, port, id, address, length, data=[], error=0): - result = COMM_TX_FAIL - - txpacket = [0] * 14 - rxpacket = [] + def readTxRx(self, port, id, address, length): + error = 0 + result = COMM_TX_FAIL - if len(data) != 0: - data = [] + txpacket = [0] * 14 + rxpacket = None + data = [] if id >= BROADCAST_ID: return data, COMM_NOT_AVAILABLE, error - txpacket[PKT_ID] = 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) + txpacket[PKT_ID] = 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, rxpacket, error) + rxpacket, result, error = self.txRxPacket(port, txpacket) if result == COMM_SUCCESS: - if error != 0: - error = rxpacket[PKT_ERROR] + error = rxpacket[PKT_ERROR] data.extend(rxpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length]) del rxpacket[:]; del rxpacket return data, result, error - def read1ByteTx(self, port, id, address): - return self.readTx(port, id, address, 1) - def read1ByteRx(self, port, id, data=[], error=0): - data_read = [0] - data_read, result, error = self.readRx(port, id, 1, data_read, error) - if result == COMM_SUCCESS: - data = data_read[0] - return data, result, error - def read1ByteTxRx(self, port, id, address, data=[], error=0): - data_read = [0] - data_read, result, error = self.readTxRx(port, id, address, 1, data_read, error) - if result == COMM_SUCCESS: - data = data_read[0] - return data, result, error - - def read2ByteTx(self, port, id, address): - return self.readTx(port, id, address, 2) - def read2ByteRx(self, port, id, data=[], error=0): - data_read = [0] * 2 - data_read, result, error = self.readRx(port, id, 2, data_read, error) - if result == COMM_SUCCESS: - data = DXL_MAKEWORD(data_read[0], data_read[1]) - return data, result, error - def read2ByteTxRx(self, port, id, address, data=[], error=0): - data_read = [0] * 2 - data_read, result, error = self.readTxRx(port, id, address, 2, data_read, error) - if result == COMM_SUCCESS: - data = DXL_MAKEWORD(data_read[0], data_read[1]) - return data, result, error - - def read4ByteTx(self, port, id, address): - return self.readTx(port, id, address, 4) - def read4ByteRx(self, port, id, data=[], error=0): - data_read = [0] * 4 - data_read, result, error = self.readRx(port, id, 4, data_read, error) - if result == COMM_SUCCESS: - data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) - return data, result, error - def read4ByteTxRx(self, port, id, address, data=[], error=0): - data_read = [0] * 4 - data_read, result, error = self.readTxRx(port, id, address, 4, data_read, error) - if result == COMM_SUCCESS: - data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])) - return data, result, error - - - def writeTxOnly(self, port, id, address, length, data): - result = COMM_TX_FAIL - - txpacket = [0] * (length + 12) + 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): + result = COMM_TX_FAIL - txpacket[PKT_ID] = 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 = [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 - del txpacket[:]; del txpacket return result - def writeTxRx(self, port, id, address, length, data, error=0): - result = COMM_TX_FAIL + def writeTxRx(self, port, dxl_id, address, length, data): + result = COMM_TX_FAIL - txpacket = [0] * (length + 12) - rxpacket = [] + txpacket = [0] * (length + 12) + rxpacket = None - txpacket[PKT_ID] = 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_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, rxpacket, error) + rxpacket, result, error = self.txRxPacket(port, txpacket) - del txpacket[:]; del txpacket + del rxpacket[:]; del rxpacket return result, error - def write1ByteTxOnly(self, port, id, address, data): + def write1ByteTxOnly(self, port, dxl_id, address, data): data_write = [data] - return self.writeTxOnly(port, id, address, 1, data_write) - def write1ByteTxRx(self, port, id, address, data, error=0): + 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, id, address, 1, data_write) + return self.writeTxRx(port, dxl_id, address, 1, data_write) - def write2ByteTxOnly(self, port, id, address, data): + def write2ByteTxOnly(self, port, dxl_id, address, data): data_write = [DXL_LOBYTE(data), DXL_HIBYTE(data)] - return self.writeTxOnly(port, id, address, 2, data_write) - def write2ByteTxRx(self, port, id, address, data, error=0): + 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, id, address, 2, data_write) - - def write4ByteTxOnly(self, port, 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, id, address, 4, data_write) - def write4ByteTxRx(self, port, id, address, data, error=0): - 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, id, address, 4, data_write) - - def regWriteTxOnly(self, port, id, address, length, data): - result = COMM_TX_FAIL + 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): + result = COMM_TX_FAIL - txpacket = [0] * (length + 12) + txpacket = [0] * (length + 12) - txpacket[PKT_ID] = 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_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 - del txpacket[:]; del txpacket return result - def regWriteTxRx(self, port, id, address, length, data, error=0): - result = COMM_TX_FAIL + def regWriteTxRx(self, port, dxl_id, address, length, data): + result = COMM_TX_FAIL + error = 0 - txpacket = [0] * (length + 12) - rxpacket = [] + txpacket = [0] * (length + 12) - txpacket[PKT_ID] = 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_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, rxpacket, error) + _, result, error = self.txRxPacket(port, txpacket) - del txpacket[:]; del txpacket return result, error def syncReadTx(self, port, start_address, data_length, param, param_length): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * (param_length + 14) + 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_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] @@ -724,41 +717,39 @@ def syncReadTx(self, port, start_address, data_length, param, param_length): if result == COMM_SUCCESS: port.setPacketTimeout((11 + data_length) * param_length) - del txpacket[:]; del txpacket return result def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * (param_length + 14) + 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_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) + _, result, _ = self.txRxPacket(port, txpacket) - del txpacket[:]; del txpacket return result def bulkReadTx(self, port, param, param_length): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * (param_length + 10) + 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_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] @@ -771,23 +762,21 @@ def bulkReadTx(self, port, param, param_length): i += 5 port.setPacketTimeout(wait_length) - del txpacket[:]; del txpacket return result def bulkWriteTxOnly(self, port, param, param_length): - result = COMM_TX_FAIL + result = COMM_TX_FAIL - txpacket = [0] * (param_length + 10) + 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_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) + _, result, _ = self.txRxPacket(port, txpacket) - del txpacket[:]; del txpacket return result diff --git a/python/src/dynamixel_sdk/robotis_def.py b/python/src/dynamixel_sdk/robotis_def.py index 45ff114e..58d8dfb0 100644 --- a/python/src/dynamixel_sdk/robotis_def.py +++ b/python/src/dynamixel_sdk/robotis_def.py @@ -19,34 +19,34 @@ # Author: Ryu Woon Jung (Leon) -BROADCAST_ID = 0xFE # 254 -MAX_ID = 0xFC # 252 +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 +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 +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 # +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): From 53538e55f448a4c4ecc0ee6ceb4bf4d914752248 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 4 May 2018 14:14:19 +0200 Subject: [PATCH 17/25] PEP 8 styling and small fixes - fixed syntax "not _ in" as "_ not in" - removed not needed variable declaration - removed not needed local list cleanup - fixed PEP8 spacing rules - renamed "id" params (python dislikes the custom use of "id" name) - propagate fixes from port_handler_linux to win and mac versions Signed-off-by: Patrick Roncagliolo --- python/src/dynamixel_sdk/group_bulk_read.py | 68 ++-- python/src/dynamixel_sdk/group_bulk_write.py | 41 +- python/src/dynamixel_sdk/group_sync_read.py | 48 +-- python/src/dynamixel_sdk/group_sync_write.py | 36 +- python/src/dynamixel_sdk/packet_handler.py | 3 +- .../src/dynamixel_sdk/port_handler_linux.py | 20 +- python/src/dynamixel_sdk/port_handler_mac.py | 28 +- .../src/dynamixel_sdk/port_handler_windows.py | 28 +- .../dynamixel_sdk/protocol1_packet_handler.py | 125 +++---- .../dynamixel_sdk/protocol2_packet_handler.py | 351 +++++++++--------- python/src/dynamixel_sdk/robotis_def.py | 52 +-- 11 files changed, 397 insertions(+), 403 deletions(-) diff --git a/python/src/dynamixel_sdk/group_bulk_read.py b/python/src/dynamixel_sdk/group_bulk_read.py index 5e91306c..2e17ffb3 100644 --- a/python/src/dynamixel_sdk/group_bulk_read.py +++ b/python/src/dynamixel_sdk/group_bulk_read.py @@ -25,6 +25,7 @@ PARAM_NUM_ADDRESS = 1 PARAM_NUM_LENGTH = 2 + class GroupBulkRead: def __init__(self, port, ph): self.port = port @@ -43,33 +44,33 @@ def makeParam(self): self.param = [] - for id in self.data_dict: + for dxl_id in self.data_dict: if self.ph.getProtocolVersion() == 1.0: - self.param.append(self.data_dict[id][2]) # LEN - self.param.append(id) # ID - self.param.append(self.data_dict[id][1]) # ADDR + 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(id) # ID - self.param.append(DXL_LOBYTE(self.data_dict[id][1])) # ADDR_L - self.param.append(DXL_HIBYTE(self.data_dict[id][1])) # ADDR_H - self.param.append(DXL_LOBYTE(self.data_dict[id][2])) # LEN_L - self.param.append(DXL_HIBYTE(self.data_dict[id][2])) # LEN_H - - def addParam(self, id, start_address, data_length): - if id in self.data_dict: # id already exist + 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[id] = [data, start_address, data_length] + data = [] # [0] * data_length + self.data_dict[dxl_id] = [data, start_address, data_length] self.is_param_changed = True return True - def removeParam(self, id): - if not id in self.data_dict: # NOT exist + def removeParam(self, dxl_id): + if dxl_id not in self.data_dict: # NOT exist return - del self.data_dict[id] + del self.data_dict[dxl_id] self.is_param_changed = True @@ -81,7 +82,7 @@ def txPacket(self): if len(self.data_dict.keys()) == 0: return COMM_NOT_AVAILABLE - if self.is_param_changed == True or not self.param: + if self.is_param_changed is True or not self.param: self.makeParam() if self.ph.getProtocolVersion() == 1.0: @@ -97,8 +98,9 @@ def rxPacket(self): if len(self.data_dict.keys()) == 0: return COMM_NOT_AVAILABLE - for id in self.data_dict: - self.data_dict[id][PARAM_NUM_DATA], result, _ = self.ph.readRx(self.port, id, self.data_dict[id][PARAM_NUM_LENGTH]) + 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 @@ -108,36 +110,38 @@ def rxPacket(self): return result def txRxPacket(self): - result = COMM_TX_FAIL - result = self.txPacket() if result != COMM_SUCCESS: return result return self.rxPacket() - def isAvailable(self, id, address, data_length): - if self.last_result == False or not id in self.data_dict: + 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[id][PARAM_NUM_ADDRESS] + start_addr = self.data_dict[dxl_id][PARAM_NUM_ADDRESS] - if (address < start_addr) or (start_addr + self.data_dict[id][PARAM_NUM_LENGTH] - data_length < 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, id, address, data_length): - if self.isAvailable(id, address, data_length) == False: + def getData(self, dxl_id, address, data_length): + if not self.isAvailable(dxl_id, address, data_length): return 0 - start_addr = self.data_dict[id][PARAM_NUM_ADDRESS] + start_addr = self.data_dict[dxl_id][PARAM_NUM_ADDRESS] if data_length == 1: - return self.data_dict[id][PARAM_NUM_DATA][address - start_addr] + return self.data_dict[dxl_id][PARAM_NUM_DATA][address - start_addr] elif data_length == 2: - return DXL_MAKEWORD(self.data_dict[id][PARAM_NUM_DATA][address - start_addr], self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 1]) + 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[id][PARAM_NUM_DATA][address - start_addr + 0], self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 1]), DXL_MAKEWORD(self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 2], self.data_dict[id][PARAM_NUM_DATA][address - start_addr + 3])) + 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 index 7b1ee0f9..fa7bbb79 100644 --- a/python/src/dynamixel_sdk/group_bulk_write.py +++ b/python/src/dynamixel_sdk/group_bulk_write.py @@ -21,6 +21,7 @@ from .robotis_def import * + class GroupBulkWrite: def __init__(self, port, ph): self.port = port @@ -38,55 +39,55 @@ def makeParam(self): self.param = [] - for id in self.data_list: - if not self.data_list[id]: + for dxl_id in self.data_list: + if not self.data_list[dxl_id]: return - self.param.append(id) - self.param.append(DXL_LOBYTE(self.data_list[id][1])) - self.param.append(DXL_HIBYTE(self.data_list[id][1])) - self.param.append(DXL_LOBYTE(self.data_list[id][2])) - self.param.append(DXL_HIBYTE(self.data_list[id][2])) + 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[id][0]) + self.param.extend(self.data_list[dxl_id][0]) - def addParam(self, id, start_address, data_length, data): + def addParam(self, dxl_id, start_address, data_length, data): if self.ph.getProtocolVersion() == 1.0: return False - if id in self.data_list: # id already exist + if dxl_id in self.data_list: # dxl_id already exist return False - if len(data) > data_length: # input data is longer than set + if len(data) > data_length: # input data is longer than set return False - self.data_list[id] = [data, start_address, data_length] + self.data_list[dxl_id] = [data, start_address, data_length] self.is_param_changed = True return True - def removeParam(self, id): + def removeParam(self, dxl_id): if self.ph.getProtocolVersion() == 1.0: return - if not id in self.data_list: # NOT exist + if dxl_id not in self.data_list: # NOT exist return - del self.data_list[id] + del self.data_list[dxl_id] self.is_param_changed = True - def changeParam(self, id, start_address, data_length, data): + def changeParam(self, dxl_id, start_address, data_length, data): if self.ph.getProtocolVersion() == 1.0: return False - if not id in self.data_list: # NOT exist + if dxl_id not in self.data_list: # NOT exist return False - if len(data) > data_length: # input data is longer than set + if len(data) > data_length: # input data is longer than set return False - self.data_list[id] = [data, start_address, data_length] + self.data_list[dxl_id] = [data, start_address, data_length] self.is_param_changed = True return True @@ -102,7 +103,7 @@ 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 == True or len(self.param) == 0: + 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 index c58dbbb7..8a3441a5 100644 --- a/python/src/dynamixel_sdk/group_sync_read.py +++ b/python/src/dynamixel_sdk/group_sync_read.py @@ -21,6 +21,7 @@ from .robotis_def import * + class GroupSyncRead: def __init__(self, port, ph, start_address, data_length): self.port = port @@ -39,34 +40,34 @@ def makeParam(self): if self.ph.getProtocolVersion() == 1.0: return - if not self.data_dict: #len(self.data_dict.keys()) == 0: + if not self.data_dict: # len(self.data_dict.keys()) == 0: return self.param = [] - for id in self.data_dict: - self.param.append(id) + for dxl_id in self.data_dict: + self.param.append(dxl_id) - def addParam(self, id): + def addParam(self, dxl_id): if self.ph.getProtocolVersion() == 1.0: return False - if id in self.data_dict: # id already exist + if dxl_id in self.data_dict: # dxl_id already exist return False - self.data_dict[id] = [] # [0] * self.data_length + self.data_dict[dxl_id] = [] # [0] * self.data_length self.is_param_changed = True return True - def removeParam(self, id): + def removeParam(self, dxl_id): if self.ph.getProtocolVersion() == 1.0: return - if not id in self.data_dict: # NOT exist + if dxl_id not in self.data_dict: # NOT exist return - del self.data_dict[id] + del self.data_dict[dxl_id] self.is_param_changed = True @@ -80,10 +81,11 @@ 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 == True or not self.param: + 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) + 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 @@ -96,8 +98,8 @@ def rxPacket(self): if len(self.data_dict.keys()) == 0: return COMM_NOT_AVAILABLE - for id in self.data_dict: - self.data_dict[id], result, _ = self.ph.readRx(self.port, id, self.data_length) + 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 @@ -110,16 +112,14 @@ def txRxPacket(self): if self.ph.getProtocolVersion() == 1.0: return COMM_NOT_AVAILABLE - result = COMM_TX_FAIL - result = self.txPacket() if result != COMM_SUCCESS: return result return self.rxPacket() - def isAvailable(self, id, address, data_length): - if self.ph.getProtocolVersion() == 1.0 or self.last_result == False or not id in self.data_dict: + 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): @@ -127,15 +127,19 @@ def isAvailable(self, id, address, data_length): return True - def getData(self, id, address, data_length): - if self.isAvailable(id, address, data_length) == False: + 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[id][address - self.start_address] + return self.data_dict[dxl_id][address - self.start_address] elif data_length == 2: - return DXL_MAKEWORD(self.data_dict[id][address - self.start_address], self.data_dict[id][address - self.start_address + 1]) + 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[id][address - self.start_address + 0], self.data_dict[id][address - self.start_address + 1]), DXL_MAKEWORD(self.data_dict[id][address - self.start_address + 2], self.data_dict[id][address - self.start_address + 3])) + 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 index be6c2775..c9314e8f 100644 --- a/python/src/dynamixel_sdk/group_sync_write.py +++ b/python/src/dynamixel_sdk/group_sync_write.py @@ -21,6 +21,7 @@ from .robotis_def import * + class GroupSyncWrite: def __init__(self, port, ph, start_address, data_length): self.port = port @@ -40,41 +41,41 @@ def makeParam(self): self.param = [] - for id in self.data_dict: - if not self.data_dict[id]: + for dxl_id in self.data_dict: + if not self.data_dict[dxl_id]: return - self.param.append(id) - self.param.extend(self.data_dict[id]) + self.param.append(dxl_id) + self.param.extend(self.data_dict[dxl_id]) - def addParam(self, id, data): - if (id in self.data_dict): # id already exist + 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 + if len(data) > self.data_length: # input data is longer than set return False - self.data_dict[id] = data + self.data_dict[dxl_id] = data self.is_param_changed = True return True - def removeParam(self, id): - if not id in self.data_dict: # NOT exist + def removeParam(self, dxl_id): + if dxl_id not in self.data_dict: # NOT exist return - del self.data_dict[id] + del self.data_dict[dxl_id] self.is_param_changed = True - def changeParam(self, id, data): - if not id in self.data_dict: # NOT exist + 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 + if len(data) > self.data_length: # input data is longer than set return False - self.data_dict[id] = data + self.data_dict[dxl_id] = data self.is_param_changed = True return True @@ -86,7 +87,8 @@ def txPacket(self): if len(self.data_dict.keys()) == 0: return COMM_NOT_AVAILABLE - if self.is_param_changed == True or not self.param: + 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)) + 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 index abfbfde2..e6620c80 100644 --- a/python/src/dynamixel_sdk/packet_handler.py +++ b/python/src/dynamixel_sdk/packet_handler.py @@ -22,8 +22,9 @@ 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 + # FIXME: float or int-to-float comparison can generate weird behaviour if protocol_version == 1.0: return Protocol1PacketHandler() elif protocol_version == 2.0: diff --git a/python/src/dynamixel_sdk/port_handler_linux.py b/python/src/dynamixel_sdk/port_handler_linux.py index 2beed368..9e312616 100644 --- a/python/src/dynamixel_sdk/port_handler_linux.py +++ b/python/src/dynamixel_sdk/port_handler_linux.py @@ -25,6 +25,7 @@ LATENCY_TIMER = 16 DEFAULT_BAUDRATE = 1000000 + class PortHandlerLinux(object): def __init__(self, port_name): self.is_open = False @@ -59,7 +60,7 @@ def setBaudRate(self, baudrate): if baud <= 0: # self.setupPort(38400) # self.baudrate = baudrate - return False #TODO: setCustomBaudrate(baudrate) + return False # TODO: setCustomBaudrate(baudrate) else: self.baudrate = baudrate return self.setupPort(baud) @@ -95,23 +96,23 @@ def getCurrentTime(self): return round(time.time() * 1000000000) / 1000000.0 def getTimeSinceStart(self): - time = self.getCurrentTime() - self.packet_start_time - if time < 0.0: + time_since = self.getCurrentTime() - self.packet_start_time + if time_since < 0.0: self.packet_start_time = self.getCurrentTime() - return time + 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, + port=self.port_name, + baudrate=self.baudrate, # parity = serial.PARITY_ODD, # stopbits = serial.STOPBITS_TWO, - bytesize = serial.EIGHTBITS, - timeout = 0 + bytesize=serial.EIGHTBITS, + timeout=0 ) self.is_open = True @@ -123,7 +124,8 @@ def setupPort(self, cflag_baud): return True def getCFlagBaud(self, baudrate): - if baudrate in [9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1152000, 2000000, 2500000, 3000000, 3500000, 4000000]: + 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/port_handler_mac.py b/python/src/dynamixel_sdk/port_handler_mac.py index 90396303..f586ca48 100644 --- a/python/src/dynamixel_sdk/port_handler_mac.py +++ b/python/src/dynamixel_sdk/port_handler_mac.py @@ -25,6 +25,7 @@ LATENCY_TIMER = 16 DEFAULT_BAUDRATE = 1000000 + class PortHandlerMac(object): def __init__(self, port_name): self.is_open = False @@ -34,7 +35,8 @@ def __init__(self, port_name): self.tx_time_per_byte = 0.0 self.is_using = False - self.setPortName(port_name) + self.port_name = port_name + self.ser = None def openPort(self): return self.setBaudRate(self.baudrate) @@ -68,18 +70,16 @@ def getBytesAvailable(self): return self.ser.in_waiting def readPort(self, length): - read_bytes = [] - read_bytes.extend([ord(ch) for ch in self.ser.read(length)]) - return read_bytes + 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 * self.packet_length) + (LATENCY_TIMER * 2.0) + 2.0 + self.packet_timeout = (self.tx_time_per_byte * packet_length) + (LATENCY_TIMER * 2.0) + 2.0 - def setPacketTimeout(self, msec): + def setPacketTimeoutMillis(self, msec): self.packet_start_time = self.getCurrentTime() self.packet_timeout = msec @@ -94,23 +94,23 @@ def getCurrentTime(self): return round(time.time() * 1000000000) / 1000000.0 def getTimeSinceStart(self): - time = self.getCurrentTime() - self.packet_start_time - if time < 0.0: - packet_start_time = self.getCurrentTime() + time_since = self.getCurrentTime() - self.packet_start_time + if time_since < 0.0: + self.packet_start_time = self.getCurrentTime() - return time + 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, + port=self.port_name, + baudrate=self.baudrate, # parity = serial.PARITY_ODD, # stopbits = serial.STOPBITS_TWO, - bytesize = serial.EIGHTBITS, - timeout = 0 + bytesize=serial.EIGHTBITS, + timeout=0 ) self.is_open = True diff --git a/python/src/dynamixel_sdk/port_handler_windows.py b/python/src/dynamixel_sdk/port_handler_windows.py index 309b1061..cb1093b7 100644 --- a/python/src/dynamixel_sdk/port_handler_windows.py +++ b/python/src/dynamixel_sdk/port_handler_windows.py @@ -25,6 +25,7 @@ LATENCY_TIMER = 1 DEFAULT_BAUDRATE = 1000000 + class PortHandlerWindows(object): def __init__(self, port_name): self.is_open = False @@ -34,7 +35,8 @@ def __init__(self, port_name): self.tx_time_per_byte = 0.0 self.is_using = False - self.setPortName(port_name) + self.port_name = port_name + self.ser = None def openPort(self): return self.setBaudRate(self.baudrate) @@ -68,9 +70,7 @@ def getBytesAvailable(self): return self.ser.in_waiting def readPort(self, length): - # read_bytes = [] - read_bytes = self.ser.readline() - return read_bytes + return [ord(ch) for ch in self.ser.read(length)] def writePort(self, packet): res = self.ser.write(packet) @@ -78,9 +78,9 @@ def writePort(self, packet): def setPacketTimeout(self, packet_length): self.packet_start_time = self.getCurrentTime() - self.packet_timeout = (self.tx_time_per_byte * self.packet_length) + (LATENCY_TIMER * 2.0) + 2.0 + self.packet_timeout = (self.tx_time_per_byte * packet_length) + (LATENCY_TIMER * 2.0) + 2.0 - def setPacketTimeout(self, msec): + def setPacketTimeoutMillis(self, msec): self.packet_start_time = self.getCurrentTime() self.packet_timeout = msec @@ -95,23 +95,23 @@ def getCurrentTime(self): return round(time.time() * 1000000000) / 1000000.0 def getTimeSinceStart(self): - time = self.getCurrentTime() - self.packet_start_time - if time < 0.0: - packet_start_time = self.getCurrentTime() + time_since = self.getCurrentTime() - self.packet_start_time + if time_since < 0.0: + self.packet_start_time = self.getCurrentTime() - return time + 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, + port=self.port_name, + baudrate=self.baudrate, # parity = serial.PARITY_ODD, # stopbits = serial.STOPBITS_TWO, - bytesize = serial.EIGHTBITS, - timeout = 0 + bytesize=serial.EIGHTBITS, + timeout=0 ) self.is_open = True diff --git a/python/src/dynamixel_sdk/protocol1_packet_handler.py b/python/src/dynamixel_sdk/protocol1_packet_handler.py index 164e4bb4..a952db7d 100644 --- a/python/src/dynamixel_sdk/protocol1_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -34,13 +34,14 @@ 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. +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): @@ -94,7 +95,7 @@ def getRxPacketError(self, error): def txPacket(self, port, txpacket): checksum = 0 - total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH + total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH if port.is_using: return COMM_PORT_BUSY @@ -110,7 +111,7 @@ def txPacket(self, port, txpacket): txpacket[PKT_HEADER1] = 0xFF # add a checksum to the packet - for idx in range(2, total_packet_length - 1): # except header, checksum + for idx in range(2, total_packet_length - 1): # except header, checksum checksum += txpacket[idx] txpacket[total_packet_length - 1] = ~checksum & 0xFF @@ -130,7 +131,7 @@ def rxPacket(self, port): result = COMM_TX_FAIL checksum = 0 rx_length = 0 - wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) + wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) while True: rxpacket.extend(port.readPort(wait_length - rx_length)) @@ -141,8 +142,9 @@ def rxPacket(self, port): 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): + 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] @@ -166,7 +168,7 @@ def rxPacket(self, port): continue # calculate checksum - for i in range(2, wait_length - 1): # except header, checksum + for i in range(2, wait_length - 1): # except header, checksum checksum += rxpacket[i] checksum = ~checksum & 0xFF @@ -178,8 +180,8 @@ def rxPacket(self, port): break else: - #remove unnecessary packets - del rxpacket[0 : idx] + # remove unnecessary packets + del rxpacket[0: idx] rx_length -= idx else: @@ -198,7 +200,6 @@ def rxPacket(self, port): # NOT for BulkRead def txRxPacket(self, port, txpacket): rxpacket = None - result = COMM_TX_FAIL error = 0 # tx packet @@ -220,7 +221,7 @@ def txRxPacket(self, port, txpacket): if txpacket[PKT_INSTRUCTION] == INST_READ: port.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6) else: - port.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + port.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM # rx packet while True: @@ -234,12 +235,10 @@ def txRxPacket(self, port, txpacket): return rxpacket, result, error def ping(self, port, dxl_id): - result = COMM_TX_FAIL model_number = 0 error = 0 txpacket = [0] * 6 - rxpacket = None if dxl_id >= BROADCAST_ID: return model_number, COMM_NOT_AVAILABLE, error @@ -251,11 +250,10 @@ def ping(self, port, dxl_id): 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 + 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]) - del rxpacket[:]; del rxpacket return model_number, result, error def broadcastPing(self, port): @@ -288,7 +286,6 @@ def factoryReset(self, port, dxl_id): return result, error def readTx(self, port, dxl_id, address, length): - result = COMM_TX_FAIL txpacket = [0] * 8 @@ -296,10 +293,10 @@ def readTx(self, port, dxl_id, address, length): return COMM_NOT_AVAILABLE txpacket[PKT_ID] = dxl_id - txpacket[PKT_LENGTH] = 4 + txpacket[PKT_LENGTH] = 4 txpacket[PKT_INSTRUCTION] = INST_READ - txpacket[PKT_PARAMETER0+0] = address - txpacket[PKT_PARAMETER0+1] = length + txpacket[PKT_PARAMETER0 + 0] = address + txpacket[PKT_PARAMETER0 + 1] = length result = self.txPacket(port, txpacket) @@ -325,43 +322,39 @@ def readRx(self, port, dxl_id, length): if result == COMM_SUCCESS and rxpacket[PKT_ID] == dxl_id: error = rxpacket[PKT_ERROR] - data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0 + length]) + data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) - del rxpacket[:]; del rxpacket return data, result, error def readTxRx(self, port, dxl_id, address, length): - result = COMM_TX_FAIL - error = 0 - txpacket = [0] * 8 - rxpacket = None data = [] if dxl_id >= BROADCAST_ID: - return data, COMM_NOT_AVAILABLE, error + 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 + 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]) + data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) - del rxpacket[:]; del rxpacket return data, result, error def read1ByteTx(self, port, dxl_id, address): - return self.readTx(port, dxl_id, address, 1) + 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 + 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 @@ -369,10 +362,12 @@ def read1ByteTxRx(self, port, dxl_id, address): 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 @@ -380,11 +375,13 @@ def read2ByteTxRx(self, port, dxl_id, address): 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]), @@ -392,8 +389,6 @@ def read4ByteTxRx(self, port, dxl_id, address): return data_read, result, error def writeTxOnly(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL - txpacket = [0] * (length + 7) txpacket[PKT_ID] = dxl_id @@ -401,7 +396,7 @@ def writeTxOnly(self, port, dxl_id, address, length, data): txpacket[PKT_INSTRUCTION] = INST_WRITE txpacket[PKT_PARAMETER0] = address - txpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length] = data[0 : length] + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] result = self.txPacket(port, txpacket) port.is_using = False @@ -409,25 +404,22 @@ def writeTxOnly(self, port, dxl_id, address, length, data): return result def writeTxRx(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL - txpacket = [0] * (length + 7) - rxpacket = None 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] + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] rxpacket, result, error = self.txRxPacket(port, txpacket) - del rxpacket[:]; del rxpacket 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) @@ -435,26 +427,26 @@ def write1ByteTxRx(self, port, dxl_id, address, data): 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)), + 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)), + 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): - result = COMM_TX_FAIL - txpacket = [0] * (length + 6) txpacket[PKT_ID] = dxl_id @@ -462,7 +454,7 @@ def regWriteTxOnly(self, port, dxl_id, address, length, data): txpacket[PKT_INSTRUCTION] = INST_REG_WRITE txpacket[PKT_PARAMETER0] = address - txpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length] = data[0 : length] + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] result = self.txPacket(port, txpacket) port.is_using = False @@ -470,9 +462,6 @@ def regWriteTxOnly(self, port, dxl_id, address, length, data): return result def regWriteTxRx(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL - error = 0 - txpacket = [0] * (length + 6) txpacket[PKT_ID] = dxl_id @@ -480,7 +469,7 @@ def regWriteTxRx(self, port, dxl_id, address, length, data): txpacket[PKT_INSTRUCTION] = INST_REG_WRITE txpacket[PKT_PARAMETER0] = address - txpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length] = data[0 : length] + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] _, result, error = self.txRxPacket(port, txpacket) @@ -490,35 +479,31 @@ 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): - result = COMM_TX_FAIL - 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_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 + 0] = start_address + txpacket[PKT_PARAMETER0 + 1] = data_length - txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + param_length] = param[0 : param_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): - result = COMM_TX_FAIL - 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_LENGTH] = param_length + 3 # 3: INST 0x00 ... CHKSUM txpacket[PKT_INSTRUCTION] = INST_BULK_READ - txpacket[PKT_PARAMETER0+0] = 0x00 + txpacket[PKT_PARAMETER0 + 0] = 0x00 - txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + param_length] = param[0 : param_length] + txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + param_length] = param[0: param_length] result = self.txPacket(port, txpacket) if result == COMM_SUCCESS: diff --git a/python/src/dynamixel_sdk/protocol2_packet_handler.py b/python/src/dynamixel_sdk/protocol2_packet_handler.py index 6e7769c4..a3df5ea8 100644 --- a/python/src/dynamixel_sdk/protocol2_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol2_packet_handler.py @@ -37,15 +37,16 @@ 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 +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. -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): @@ -106,43 +107,43 @@ def getRxPacketError(self, error): 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] + 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 @@ -156,14 +157,17 @@ def addStuffing(self, packet): temp = [0] * TXPACKET_MAX_LEN - temp[PKT_HEADER0 : PKT_HEADER0 + PKT_LENGTH_H + 1] = packet[PKT_HEADER0 : PKT_HEADER0 + PKT_LENGTH_H + 1] # FF FF FD XX ID LEN_L LEN_H + # 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 + 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: + 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 @@ -176,7 +180,7 @@ def addStuffing(self, packet): if packet_length_in != packet_length_out: packet = [0] * index - packet[0 : index] = temp[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) @@ -188,8 +192,9 @@ def removeStuffing(self, packet): 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): + 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 @@ -209,7 +214,7 @@ def txPacket(self, port, txpacket): if port.is_using: return COMM_PORT_BUSY port.is_using = True - + # byte stuffing for header self.addStuffing(txpacket) @@ -228,7 +233,7 @@ def txPacket(self, port, txpacket): txpacket[PKT_RESERVED] = 0x00 # add CRC16 - crc = self.updateCRC(0, txpacket, total_packet_length - 2) # 2: 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) @@ -247,7 +252,7 @@ def rxPacket(self, port): 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) + 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)) @@ -255,11 +260,14 @@ def rxPacket(self, port): 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): + 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): + 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 @@ -270,7 +278,7 @@ def rxPacket(self, port): continue if rx_length < wait_length: - if port.isPacketTimeout() == True: + if port.isPacketTimeout(): if rx_length == 0: result = COMM_RX_TIMEOUT else: @@ -279,7 +287,7 @@ def rxPacket(self, port): else: continue - crc = DXL_MAKEWORD(rxpacket[wait_length-2], rxpacket[wait_length-1]) + crc = DXL_MAKEWORD(rxpacket[wait_length - 2], rxpacket[wait_length - 1]) if self.updateCRC(0, rxpacket, wait_length - 2) == crc: result = COMM_SUCCESS @@ -289,11 +297,11 @@ def rxPacket(self, port): else: # remove unnecessary packets - del rxpacket[0 : idx] + del rxpacket[0: idx] rx_length -= idx else: - if port.isPacketTimeout() == True: + if port.isPacketTimeout(): if rx_length == 0: result = COMM_RX_TIMEOUT else: @@ -310,7 +318,6 @@ def rxPacket(self, port): # NOT for BulkRead / SyncRead instruction def txRxPacket(self, port, txpacket): rxpacket = None - result = COMM_TX_FAIL error = 0 # tx packet @@ -329,8 +336,8 @@ def txRxPacket(self, port, txpacket): 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) + 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 @@ -346,18 +353,16 @@ def txRxPacket(self, port, txpacket): return rxpacket, result, error - def ping(self, port, id): - result = COMM_TX_FAIL + def ping(self, port, dxl_id): model_number = 0 error = 0 txpacket = [0] * 10 - rxpacket = None - if id >= BROADCAST_ID: + if dxl_id >= BROADCAST_ID: return model_number, COMM_NOT_AVAILABLE, error - txpacket[PKT_ID] = id + txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH_L] = 3 txpacket[PKT_LENGTH_H] = 0 txpacket[PKT_INSTRUCTION] = INST_PING @@ -366,14 +371,12 @@ def ping(self, port, id): if result == COMM_SUCCESS: model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]) - del rxpacket[:]; del rxpacket return model_number, result, error def broadcastPing(self, port): data_list = {} - + STATUS_LENGTH = 14 - result = COMM_TX_FAIL rx_length = 0 wait_length = STATUS_LENGTH * MAX_ID @@ -398,7 +401,7 @@ def broadcastPing(self, port): rxpacket += port.readPort(wait_length - rx_length) rx_length = len(rxpacket) - if port.isPacketTimeout() == True: # or rx_length >= wait_length + if port.isPacketTimeout(): # or rx_length >= wait_length break port.is_using = False @@ -415,16 +418,18 @@ def broadcastPing(self, port): 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 + if idx == 0: # found at the beginning of the packet # verify CRC16 - crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]) + 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]] + data_list[rxpacket[PKT_ID]] = [ + DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]), + rxpacket[PKT_PARAMETER0 + 3]] - del rxpacket[0 : STATUS_LENGTH] + del rxpacket[0: STATUS_LENGTH] rx_length = rx_length - STATUS_LENGTH if rx_length == 0: @@ -434,21 +439,21 @@ def broadcastPing(self, port): result = COMM_RX_CORRUPT # remove header (0xFF 0xFF 0xFD) - del rxpacket[0 : 3] + del rxpacket[0: 3] rx_length = rx_length - 3 else: # remove unnecessary packets - del rxpacket[0 : idx] + del rxpacket[0: idx] rx_length = rx_length - idx - del rxpacket[:]; del rxpacket + # FIXME: unreachable code return data_list, result - def action(self, port, id): + def action(self, port, dxl_id): txpacket = [0] * 10 - txpacket[PKT_ID] = id + txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH_L] = 3 txpacket[PKT_LENGTH_H] = 0 txpacket[PKT_INSTRUCTION] = INST_ACTION @@ -456,12 +461,10 @@ def action(self, port, id): _, result, _ = self.txRxPacket(port, txpacket) return result - def reboot(self, port, id): - error = 0 - + def reboot(self, port, dxl_id): txpacket = [0] * 10 - txpacket[PKT_ID] = id + txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH_L] = 3 txpacket[PKT_LENGTH_H] = 0 txpacket[PKT_INSTRUCTION] = INST_REBOOT @@ -469,12 +472,10 @@ def reboot(self, port, id): _, result, error = self.txRxPacket(port, txpacket) return result, error - def factoryReset(self, port, id, option): - error = 0 - + def factoryReset(self, port, dxl_id, option): txpacket = [0] * 11 - txpacket[PKT_ID] = id + txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH_L] = 4 txpacket[PKT_LENGTH_H] = 0 txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET @@ -483,32 +484,30 @@ def factoryReset(self, port, id, option): _, result, error = self.txRxPacket(port, txpacket) return result, error - def readTx(self, port, id, address, length): - result = COMM_TX_FAIL - + def readTx(self, port, dxl_id, address, length): txpacket = [0] * 14 - if id >= BROADCAST_ID: + if dxl_id >= BROADCAST_ID: return COMM_NOT_AVAILABLE - txpacket[PKT_ID] = id + 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) + 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): + if result == COMM_SUCCESS: port.setPacketTimeout(length + 11) return result - def readRx(self, port, id, length): + def readRx(self, port, dxl_id, length): result = COMM_TX_FAIL error = 0 @@ -518,52 +517,50 @@ def readRx(self, port, id, length): while True: rxpacket, result = self.rxPacket(port) - if result != COMM_SUCCESS or rxpacket[PKT_ID] == id: + if result != COMM_SUCCESS or rxpacket[PKT_ID] == dxl_id: break - if result == COMM_SUCCESS and rxpacket[PKT_ID] == id: + if result == COMM_SUCCESS and rxpacket[PKT_ID] == dxl_id: error = rxpacket[PKT_ERROR] - data.extend(rxpacket[PKT_PARAMETER0 + 1 : PKT_PARAMETER0 + 1 + length]) + data.extend(rxpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length]) - del rxpacket[:]; del rxpacket return data, result, error - def readTxRx(self, port, id, address, length): + def readTxRx(self, port, dxl_id, address, length): error = 0 - result = COMM_TX_FAIL txpacket = [0] * 14 - rxpacket = None data = [] - if id >= BROADCAST_ID: + if dxl_id >= BROADCAST_ID: return data, COMM_NOT_AVAILABLE, error - txpacket[PKT_ID] = id + 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) + 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]) + data.extend(rxpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length]) - del rxpacket[:]; del rxpacket 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 @@ -571,10 +568,12 @@ def read1ByteTxRx(self, port, dxl_id, address): 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 @@ -582,11 +581,13 @@ def read2ByteTxRx(self, port, dxl_id, address): 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]), @@ -594,18 +595,16 @@ def read4ByteTxRx(self, port, dxl_id, address): return data_read, result, error def writeTxOnly(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL - 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_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 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) - txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + length] = data[0 : length] + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] result = self.txPacket(port, txpacket) port.is_using = False @@ -613,27 +612,24 @@ def writeTxOnly(self, port, dxl_id, address, length, data): return result def writeTxRx(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL - txpacket = [0] * (length + 12) - rxpacket = None txpacket[PKT_ID] = dxl_id - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5) - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5) + 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 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) - txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + length] = data[0 : length] + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] rxpacket, result, error = self.txRxPacket(port, txpacket) - del rxpacket[:]; del rxpacket 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) @@ -641,36 +637,36 @@ def write1ByteTxRx(self, port, dxl_id, address, data): 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)), + 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)), + 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): - result = COMM_TX_FAIL - 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_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 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) - txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + length] = data[0 : length] + txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + length] = data[0: length] result = self.txPacket(port, txpacket) port.is_using = False @@ -678,40 +674,37 @@ def regWriteTxOnly(self, port, dxl_id, address, length, data): return result def regWriteTxRx(self, port, dxl_id, address, length, data): - result = COMM_TX_FAIL - error = 0 - 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_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 + 0] = DXL_LOBYTE(address) + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(address) - txpacket[PKT_PARAMETER0 + 2 : PKT_PARAMETER0 + 2 + length] = data[0 : length] + 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): - result = COMM_TX_FAIL - 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_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 + 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] + txpacket[PKT_PARAMETER0 + 4: PKT_PARAMETER0 + 4 + param_length] = param[0: param_length] result = self.txPacket(port, txpacket) if result == COMM_SUCCESS: @@ -720,62 +713,58 @@ def syncReadTx(self, port, start_address, data_length, param, param_length): return result def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): - result = COMM_TX_FAIL - 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_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 + 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] + 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): - result = COMM_TX_FAIL - 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_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] + 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 + 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): - result = COMM_TX_FAIL - 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_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] + txpacket[PKT_PARAMETER0: PKT_PARAMETER0 + param_length] = param[0: param_length] _, result, _ = self.txRxPacket(port, txpacket) diff --git a/python/src/dynamixel_sdk/robotis_def.py b/python/src/dynamixel_sdk/robotis_def.py index 58d8dfb0..aacadd76 100644 --- a/python/src/dynamixel_sdk/robotis_def.py +++ b/python/src/dynamixel_sdk/robotis_def.py @@ -19,8 +19,8 @@ # Author: Ryu Woon Jung (Leon) -BROADCAST_ID = 0xFE # 254 -MAX_ID = 0xFC # 252 +BROADCAST_ID = 0xFE # 254 +MAX_ID = 0xFC # 252 # Instruction for DXL Protocol INST_PING = 1 @@ -29,40 +29,46 @@ INST_REG_WRITE = 4 INST_ACTION = 5 INST_FACTORY_RESET = 6 -INST_SYNC_WRITE = 131 # 0x83 -INST_BULK_READ = 146 # 0x92 +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 +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 # +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)) - + return (a & 0xFF) | ((b & 0xFF) << 8) + + def DXL_MAKEDWORD(a, b): - return ((a & 0xFFFF) | (b & 0xFFFF) << 16) + return (a & 0xFFFF) | (b & 0xFFFF) << 16 + def DXL_LOWORD(l): - return (l & 0xFFFF) + return l & 0xFFFF + def DXL_HIWORD(l): - return ((l >> 16) & 0xFFFF) + return (l >> 16) & 0xFFFF + def DXL_LOBYTE(w): - return (w & 0xFF) + return w & 0xFF + def DXL_HIBYTE(w): - return ((w >> 8) & 0xFF) \ No newline at end of file + return (w >> 8) & 0xFF From c0fc3ec52ac28b5b710f7c0ef36fde2fd45b7efa Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 4 May 2018 14:15:53 +0200 Subject: [PATCH 18/25] Port handler is truly cross-platform Signed-off-by: Patrick Roncagliolo --- python/src/dynamixel_sdk/port_handler.py | 119 +++++++++++-- .../src/dynamixel_sdk/port_handler_linux.py | 131 -------------- python/src/dynamixel_sdk/port_handler_mac.py | 162 ----------------- .../src/dynamixel_sdk/port_handler_windows.py | 163 ------------------ 4 files changed, 106 insertions(+), 469 deletions(-) delete mode 100644 python/src/dynamixel_sdk/port_handler_linux.py delete mode 100644 python/src/dynamixel_sdk/port_handler_mac.py delete mode 100644 python/src/dynamixel_sdk/port_handler_windows.py diff --git a/python/src/dynamixel_sdk/port_handler.py b/python/src/dynamixel_sdk/port_handler.py index 3e0de0ea..b274ab1c 100644 --- a/python/src/dynamixel_sdk/port_handler.py +++ b/python/src/dynamixel_sdk/port_handler.py @@ -19,20 +19,113 @@ # Author: Ryu Woon Jung (Leon) -import platform +import time +import serial -if platform.system() == 'Linux': - from .port_handler_linux import * +LATENCY_TIMER = 16 +DEFAULT_BAUDRATE = 1000000 - class PortHandler(PortHandlerLinux): - pass -elif platform.system() == 'Windows': - from .port_handler_windows import * - class PortHandler(PortHandlerWindows): - pass -elif platform.system() == 'Darwin': - from .port_handler_mac import * +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 - class PortHandler(PortHandlerMac): - pass + 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): + 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 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/port_handler_linux.py b/python/src/dynamixel_sdk/port_handler_linux.py deleted file mode 100644 index 9e312616..00000000 --- a/python/src/dynamixel_sdk/port_handler_linux.py +++ /dev/null @@ -1,131 +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 time -import serial - -LATENCY_TIMER = 16 -DEFAULT_BAUDRATE = 1000000 - - -class PortHandlerLinux(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): - 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 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/port_handler_mac.py b/python/src/dynamixel_sdk/port_handler_mac.py deleted file mode 100644 index f586ca48..00000000 --- a/python/src/dynamixel_sdk/port_handler_mac.py +++ /dev/null @@ -1,162 +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 time -import serial - -LATENCY_TIMER = 16 -DEFAULT_BAUDRATE = 1000000 - - -class PortHandlerMac(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: - return False - 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): - 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 baudrate == 9600: - return 9600 - elif baudrate == 19200: - return 19200 - elif baudrate == 38400: - return 38400 - elif baudrate == 57600: - return 57600 - elif baudrate == 115200: - return 115200 - elif baudrate == 230400: - return 230400 - elif baudrate == 460800: - return 460800 - elif baudrate == 500000: - return 500000 - elif baudrate == 576000: - return 576000 - elif baudrate == 921600: - return 921600 - elif baudrate == 1000000: - return 1000000 - elif baudrate == 1152000: - return 1152000 - elif baudrate == 1500000: - return 1500000 - elif baudrate == 2000000: - return 2000000 - elif baudrate == 2500000: - return 2500000 - elif baudrate == 3000000: - return 3000000 - elif baudrate == 3500000: - return 3500000 - elif baudrate == 4000000: - return 4000000 - else: - return -1 diff --git a/python/src/dynamixel_sdk/port_handler_windows.py b/python/src/dynamixel_sdk/port_handler_windows.py deleted file mode 100644 index cb1093b7..00000000 --- a/python/src/dynamixel_sdk/port_handler_windows.py +++ /dev/null @@ -1,163 +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 time -import serial - -LATENCY_TIMER = 1 -DEFAULT_BAUDRATE = 1000000 - - -class PortHandlerWindows(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: - return False - 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): - return [ord(ch) for ch in self.ser.read(length)] - - def writePort(self, packet): - res = self.ser.write(packet) - return res - - 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 baudrate == 9600: - return 9600 - elif baudrate == 19200: - return 19200 - elif baudrate == 38400: - return 38400 - elif baudrate == 57600: - return 57600 - elif baudrate == 115200: - return 115200 - elif baudrate == 230400: - return 230400 - elif baudrate == 460800: - return 460800 - elif baudrate == 500000: - return 500000 - elif baudrate == 576000: - return 576000 - elif baudrate == 921600: - return 921600 - elif baudrate == 1000000: - return 1000000 - elif baudrate == 1152000: - return 1152000 - elif baudrate == 1500000: - return 1500000 - elif baudrate == 2000000: - return 2000000 - elif baudrate == 2500000: - return 2500000 - elif baudrate == 3000000: - return 3000000 - elif baudrate == 3500000: - return 3500000 - elif baudrate == 4000000: - return 4000000 - else: - return -1 From 8d5fe84a4ee027b7dc5066b8b8bf6097b1c1c2c1 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 4 May 2018 14:45:33 +0200 Subject: [PATCH 19/25] Fix import and module requirement Signed-off-by: Patrick Roncagliolo --- python/setup.py | 3 ++- python/tests/protocol_combined.py | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/python/setup.py b/python/setup.py index 4985f3d2..eb430c44 100644 --- a/python/setup.py +++ b/python/setup.py @@ -1,5 +1,6 @@ from setuptools import setup, find_packages +import platform setup( name='dynamixel_sdk', @@ -12,5 +13,5 @@ url='https://github.com/ROBOTIS-GIT/DynamixelSDK', author='Leon Jung', author_email='rwjung@robotis.com', - install_requires=['pyserial'] + install_requires=['pyserial'] + (["msvcrt"] if platform.system() == 'Windows' else []) ) diff --git a/python/tests/protocol_combined.py b/python/tests/protocol_combined.py index 8e241519..a8ead995 100644 --- a/python/tests/protocol_combined.py +++ b/python/tests/protocol_combined.py @@ -49,8 +49,8 @@ def getch(): finally: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch -import dynamixel_sdk -# from dynamixel_sdk import dynamixel_sdk # Uses Dynamixel SDK library + +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 From e8fb185a15dd3438e8ad75910f650086137de1de Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 4 May 2018 16:09:27 +0200 Subject: [PATCH 20/25] Fix reg_write and action bugs reg_write was using a wrong size for its txpacket action seems to require RX after some tests on real hardware Signed-off-by: Patrick Roncagliolo --- python/src/dynamixel_sdk/protocol1_packet_handler.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/python/src/dynamixel_sdk/protocol1_packet_handler.py b/python/src/dynamixel_sdk/protocol1_packet_handler.py index a952db7d..768c6f00 100644 --- a/python/src/dynamixel_sdk/protocol1_packet_handler.py +++ b/python/src/dynamixel_sdk/protocol1_packet_handler.py @@ -116,6 +116,8 @@ def txPacket(self, port, txpacket): txpacket[total_packet_length - 1] = ~checksum & 0xFF + #print "[TxPacket] %r" % txpacket + # tx packet port.clearPort() written_packet_length = port.writePort(txpacket) @@ -195,6 +197,8 @@ def rxPacket(self, port): port.is_using = False + #print "[RxPacket] %r" % rxpacket + return rxpacket, result # NOT for BulkRead @@ -212,8 +216,7 @@ def txRxPacket(self, port, txpacket): 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): + if (txpacket[PKT_ID] == BROADCAST_ID): port.is_using = False return rxpacket, result, error @@ -447,7 +450,7 @@ def write4ByteTxRx(self, port, dxl_id, address, data): return self.writeTxRx(port, dxl_id, address, 4, data_write) def regWriteTxOnly(self, port, dxl_id, address, length, data): - txpacket = [0] * (length + 6) + txpacket = [0] * (length + 7) txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH] = length + 3 @@ -462,7 +465,7 @@ def regWriteTxOnly(self, port, dxl_id, address, length, data): return result def regWriteTxRx(self, port, dxl_id, address, length, data): - txpacket = [0] * (length + 6) + txpacket = [0] * (length + 7) txpacket[PKT_ID] = dxl_id txpacket[PKT_LENGTH] = length + 3 From e391eb5acb439da9b81adc729a3dfe7674acea7e Mon Sep 17 00:00:00 2001 From: leon Date: Mon, 14 May 2018 23:37:14 +0900 Subject: [PATCH 21/25] fixed: 2rd round reviewed python implementation and sync/bulk related bugs #185 --- python/setup.py | 2 +- python/src/dynamixel_sdk/port_handler.py | 6 +++++- python/tests/protocol2_0/ping.py | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/python/setup.py b/python/setup.py index eb430c44..1299379e 100644 --- a/python/setup.py +++ b/python/setup.py @@ -13,5 +13,5 @@ url='https://github.com/ROBOTIS-GIT/DynamixelSDK', author='Leon Jung', author_email='rwjung@robotis.com', - install_requires=['pyserial'] + (["msvcrt"] if platform.system() == 'Windows' else []) + install_requires=['pyserial'] # + (["msvcrt"] if platform.system() == 'Windows' else []) ) diff --git a/python/src/dynamixel_sdk/port_handler.py b/python/src/dynamixel_sdk/port_handler.py index b274ab1c..784020ad 100644 --- a/python/src/dynamixel_sdk/port_handler.py +++ b/python/src/dynamixel_sdk/port_handler.py @@ -21,6 +21,7 @@ import time import serial +import sys LATENCY_TIMER = 16 DEFAULT_BAUDRATE = 1000000 @@ -72,7 +73,10 @@ def getBytesAvailable(self): return self.ser.in_waiting def readPort(self, length): - return [ord(ch) for ch in self.ser.read(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) diff --git a/python/tests/protocol2_0/ping.py b/python/tests/protocol2_0/ping.py index 462ccedf..8825d5c9 100644 --- a/python/tests/protocol2_0/ping.py +++ b/python/tests/protocol2_0/ping.py @@ -53,7 +53,7 @@ def getch(): PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel # Default setting -DXL_ID = 2 # Dynamixel ID : 1 +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-*" From ec74e34a5f2ba0346eb8fe7a2a787dc699b197ce Mon Sep 17 00:00:00 2001 From: leon Date: Tue, 15 May 2018 15:59:35 +0900 Subject: [PATCH 22/25] changes: deprecated functions removed --- README.md | 6 ++-- ReleaseNote.txt | 4 +-- .../win32/dynamixel.cs | 4 --- .../win64/dynamixel.cs | 4 --- .../dynamixel_sdk/protocol1_packet_handler.h | 14 -------- .../dynamixel_sdk/protocol2_packet_handler.h | 14 -------- .../protocol1_packet_handler.cpp | 22 ------------ .../protocol2_packet_handler.cpp | 22 ------------ .../dynamixel_sdk/protocol1_packet_handler.h | 2 -- .../dynamixel_sdk/protocol2_packet_handler.h | 2 -- .../dynamixel_sdk/protocol1_packet_handler.c | 12 ------- .../dynamixel_sdk/protocol2_packet_handler.c | 12 ------- .../mac_x64/Dynamixel.java | 2 -- .../x64/Dynamixel.java | 2 -- .../x86/Dynamixel.java | 2 -- .../packet_handler/printRxPacketError.m | 36 ------------------- .../packet_handler/printTxRxResult.m | 36 ------------------- 17 files changed, 5 insertions(+), 191 deletions(-) delete mode 100644 matlab/m_basic_function/packet_handler/printRxPacketError.m delete mode 100644 matlab/m_basic_function/packet_handler/printTxRxResult.m diff --git a/README.md b/README.md index 55277744..6a942c23 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.5| -| |||(--.--.--)| +| Latest version released |||3.6.0| +| |||(18.03.15)| | 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 d047714c..e33c00ed 100644 --- a/ReleaseNote.txt +++ b/ReleaseNote.txt @@ -1,8 +1,8 @@ ============================================== - Dynamixel SDK 3.5.5 (Protocol 1.0/2.0) + Dynamixel SDK 3.6.0 (Protocol 1.0/2.0) ============================================== -- --.--.---- +- 03.15.2018 * Added : CONTRIBUTING.md added * Changes : ISSUE_TEMPLATE.md modified 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++/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++/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 8c0d7eac..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; 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/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 From 54c5766eff0554d132079b66a8504cb6127798d9 Mon Sep 17 00:00:00 2001 From: leon Date: Wed, 16 May 2018 13:30:27 +0900 Subject: [PATCH 23/25] logs changed --- Doxyfile | 2 +- README.md | 2 +- ReleaseNote.txt | 7 +++++-- c++/CHANGELOG.rst | 8 ++++++-- c++/library.properties | 2 +- c++/package.xml | 2 +- python/setup.py | 2 +- python/src/dynamixel_sdk/port_handler.py | 15 +++++++++++---- 8 files changed, 27 insertions(+), 13 deletions(-) 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 6a942c23..ac197dbd 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ | ------------- | ------------- | ------------- | ------------- | | Release date | 2010.05.16 | 2015.02.10 | 2016.03.08 | | Latest version released |||3.6.0| -| |||(18.03.15)| +| |||(18.03.16)| | OS | Linux | Windows | Linux + Windows + Mac | | Available Dynamixel models | All models | All models | All models | ||||| diff --git a/ReleaseNote.txt b/ReleaseNote.txt index e33c00ed..f7f5c2d1 100644 --- a/ReleaseNote.txt +++ b/ReleaseNote.txt @@ -2,12 +2,15 @@ Dynamixel SDK 3.6.0 (Protocol 1.0/2.0) ============================================== -- 03.15.2018 +- 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 -* Fixes : DynamixelSDK MATLAB 2017 - new typedef (int8_t / int16_t / int32_t) applied in robotis_def.h * 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++/CHANGELOG.rst b/c++/CHANGELOG.rst index 5d196cc2..0cca119d 100644 --- a/c++/CHANGELOG.rst +++ b/c++/CHANGELOG.rst @@ -2,12 +2,16 @@ Changelog for package dynamixel_sdk ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -3.5.5 (****-**-**) +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 -* Fixes : DynamixelSDK MATLAB 2017 - new typedef (int8_t / int16_t / int32_t) applied in robotis_def.h * 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 + 3.5.4 (2017-12-01) ----------- diff --git a/c++/library.properties b/c++/library.properties index 419f7bd9..b7b64bcc 100644 --- a/c++/library.properties +++ b/c++/library.properties @@ -1,5 +1,5 @@ name=DynamixelSDK -version=3.5.5 +version=3.6.0 author=ROBOTIS maintainer=ROBOTIS sentence=DynamixelSDK for Arduino diff --git a/c++/package.xml b/c++/package.xml index b06b39c1..a31b8eb1 100644 --- a/c++/package.xml +++ b/c++/package.xml @@ -1,7 +1,7 @@ dynamixel_sdk - 3.5.5 + 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/python/setup.py b/python/setup.py index 1299379e..c10954e7 100644 --- a/python/setup.py +++ b/python/setup.py @@ -13,5 +13,5 @@ url='https://github.com/ROBOTIS-GIT/DynamixelSDK', author='Leon Jung', author_email='rwjung@robotis.com', - install_requires=['pyserial'] # + (["msvcrt"] if platform.system() == 'Windows' else []) + install_requires=['pyserial'] ) diff --git a/python/src/dynamixel_sdk/port_handler.py b/python/src/dynamixel_sdk/port_handler.py index 784020ad..de555d45 100644 --- a/python/src/dynamixel_sdk/port_handler.py +++ b/python/src/dynamixel_sdk/port_handler.py @@ -22,6 +22,7 @@ import time import serial import sys +import platform LATENCY_TIMER = 16 DEFAULT_BAUDRATE = 1000000 @@ -128,8 +129,14 @@ def setupPort(self, cflag_baud): return True def getCFlagBaud(self, baudrate): - if baudrate in [9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1152000, - 2000000, 2500000, 3000000, 3500000, 4000000]: - return baudrate + if platform.system() == 'Darwin': + if baudrate in [9600, 19200, 38400, 57600, 115200]: + return baudrate + else: + return -1 else: - return -1 + if baudrate in [9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1152000, + 2000000, 2500000, 3000000, 3500000, 4000000]: + return baudrate + else: + return -1 From 45fa41931fd854d4d0958d2354b8f65a72b2fedf Mon Sep 17 00:00:00 2001 From: leon Date: Wed, 16 May 2018 13:32:25 +0900 Subject: [PATCH 24/25] logs changed --- c++/CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/c++/CHANGELOG.rst b/c++/CHANGELOG.rst index 0cca119d..0669204f 100644 --- a/c++/CHANGELOG.rst +++ b/c++/CHANGELOG.rst @@ -11,7 +11,7 @@ Changelog for package dynamixel_sdk * 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) ----------- From c55b759fb412d48102793f0a0d0191fc6c5e65ac Mon Sep 17 00:00:00 2001 From: leon Date: Wed, 16 May 2018 13:45:25 +0900 Subject: [PATCH 25/25] deprecated functions in packethandler.h remained was removed --- c++/include/dynamixel_sdk/packet_handler.h | 14 -------------- c/include/dynamixel_sdk/packet_handler.h | 2 -- 2 files changed, 16 deletions(-) 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/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);