From 49c8f0fc012c6f9a63a663d93130cd1153e258a5 Mon Sep 17 00:00:00 2001 From: NathanDuPont Date: Thu, 18 Jul 2024 00:26:35 -0700 Subject: [PATCH] Adding dynamixel SDK --- src/drivers/dynamixel/BUILD.bazel | 72 ++ src/drivers/dynamixel/README.md | 3 + src/drivers/dynamixel/dynamixel_sdk.h | 34 + src/drivers/dynamixel/group_bulk_read.cpp | 223 ++++ src/drivers/dynamixel/group_bulk_read.h | 153 +++ src/drivers/dynamixel/group_bulk_write.cpp | 149 +++ src/drivers/dynamixel/group_bulk_write.h | 110 ++ .../dynamixel/group_fast_bulk_read.cpp | 116 ++ src/drivers/dynamixel/group_fast_bulk_read.h | 44 + .../dynamixel/group_fast_sync_read.cpp | 97 ++ src/drivers/dynamixel/group_fast_sync_read.h | 42 + src/drivers/dynamixel/group_handler.cpp | 33 + src/drivers/dynamixel/group_handler.h | 53 + src/drivers/dynamixel/group_sync_read.cpp | 201 +++ src/drivers/dynamixel/group_sync_read.h | 159 +++ src/drivers/dynamixel/group_sync_write.cpp | 130 ++ src/drivers/dynamixel/group_sync_write.h | 105 ++ src/drivers/dynamixel/packet_handler.cpp | 49 + src/drivers/dynamixel/packet_handler.h | 710 ++++++++++ src/drivers/dynamixel/port_handler.cpp | 48 + src/drivers/dynamixel/port_handler.h | 190 +++ .../dynamixel/port_handler_arduino.cpp | 269 ++++ src/drivers/dynamixel/port_handler_arduino.h | 197 +++ src/drivers/dynamixel/port_handler_linux.cpp | 288 +++++ src/drivers/dynamixel/port_handler_linux.h | 183 +++ src/drivers/dynamixel/port_handler_mac.cpp | 238 ++++ src/drivers/dynamixel/port_handler_mac.h | 184 +++ .../dynamixel/port_handler_windows.cpp | 216 ++++ src/drivers/dynamixel/port_handler_windows.h | 185 +++ .../dynamixel/protocol1_packet_handler.cpp | 766 +++++++++++ .../dynamixel/protocol1_packet_handler.h | 671 ++++++++++ .../dynamixel/protocol2_packet_handler.cpp | 1151 +++++++++++++++++ .../dynamixel/protocol2_packet_handler.h | 695 ++++++++++ 33 files changed, 7764 insertions(+) create mode 100644 src/drivers/dynamixel/BUILD.bazel create mode 100644 src/drivers/dynamixel/README.md create mode 100644 src/drivers/dynamixel/dynamixel_sdk.h create mode 100644 src/drivers/dynamixel/group_bulk_read.cpp create mode 100644 src/drivers/dynamixel/group_bulk_read.h create mode 100644 src/drivers/dynamixel/group_bulk_write.cpp create mode 100644 src/drivers/dynamixel/group_bulk_write.h create mode 100644 src/drivers/dynamixel/group_fast_bulk_read.cpp create mode 100644 src/drivers/dynamixel/group_fast_bulk_read.h create mode 100644 src/drivers/dynamixel/group_fast_sync_read.cpp create mode 100644 src/drivers/dynamixel/group_fast_sync_read.h create mode 100644 src/drivers/dynamixel/group_handler.cpp create mode 100644 src/drivers/dynamixel/group_handler.h create mode 100644 src/drivers/dynamixel/group_sync_read.cpp create mode 100644 src/drivers/dynamixel/group_sync_read.h create mode 100644 src/drivers/dynamixel/group_sync_write.cpp create mode 100644 src/drivers/dynamixel/group_sync_write.h create mode 100644 src/drivers/dynamixel/packet_handler.cpp create mode 100644 src/drivers/dynamixel/packet_handler.h create mode 100644 src/drivers/dynamixel/port_handler.cpp create mode 100644 src/drivers/dynamixel/port_handler.h create mode 100644 src/drivers/dynamixel/port_handler_arduino.cpp create mode 100644 src/drivers/dynamixel/port_handler_arduino.h create mode 100644 src/drivers/dynamixel/port_handler_linux.cpp create mode 100644 src/drivers/dynamixel/port_handler_linux.h create mode 100644 src/drivers/dynamixel/port_handler_mac.cpp create mode 100644 src/drivers/dynamixel/port_handler_mac.h create mode 100644 src/drivers/dynamixel/port_handler_windows.cpp create mode 100644 src/drivers/dynamixel/port_handler_windows.h create mode 100644 src/drivers/dynamixel/protocol1_packet_handler.cpp create mode 100644 src/drivers/dynamixel/protocol1_packet_handler.h create mode 100644 src/drivers/dynamixel/protocol2_packet_handler.cpp create mode 100644 src/drivers/dynamixel/protocol2_packet_handler.h diff --git a/src/drivers/dynamixel/BUILD.bazel b/src/drivers/dynamixel/BUILD.bazel new file mode 100644 index 0000000..5cc784b --- /dev/null +++ b/src/drivers/dynamixel/BUILD.bazel @@ -0,0 +1,72 @@ +cc_library( + name = "port_handler_lib", + srcs = [ + "port_handler.cpp", + "port_handler_arduino.cpp", + "port_handler_linux.cpp", + "port_handler_mac.cpp", + "port_handler_windows.cpp", + ], + hdrs = [ + "port_handler.h", + "port_handler_arduino.h", + "port_handler_linux.h", + "port_handler_mac.h", + "port_handler_windows.h", + ], +) + +cc_library( + name = "packet_handler_lib", + srcs = [ + "packet_handler.cpp", + "protocol1_packet_handler.cpp", + "protocol2_packet_handler.cpp", + ], + hdrs = [ + "packet_handler.h", + "protocol1_packet_handler.h", + "protocol2_packet_handler.h", + ], + deps = [ + ":port_handler_lib", + ], +) + +cc_library( + name = "group_io_lib", + srcs = [ + "group_bulk_read.cpp", + "group_bulk_write.cpp", + "group_fast_bulk_read.cpp", + "group_fast_sync_read.cpp", + "group_handler.cpp", + "group_sync_read.cpp", + "group_sync_write.cpp", + ], + hdrs = [ + "group_bulk_read.h", + "group_bulk_write.h", + "group_fast_bulk_read.h", + "group_fast_sync_read.h", + "group_handler.h", + "group_sync_read.h", + "group_sync_write.h", + ], + deps = [ + ":packet_handler_lib", + ":port_handler_lib", + ], +) + +cc_library( + name = "sdk", + hdrs = [ + "dynamixel_sdk.h", + ], + deps = [ + ":group_io_lib", + ":packet_handler_lib", + ":port_handler_lib", + ], +) diff --git a/src/drivers/dynamixel/README.md b/src/drivers/dynamixel/README.md new file mode 100644 index 0000000..f46023a --- /dev/null +++ b/src/drivers/dynamixel/README.md @@ -0,0 +1,3 @@ +# Dynamixel SDK C++ Implementation + +Copied from [(github)](https://github.com/ROBOTIS-GIT/DynamixelSDK/tree/master/c%2B%2B). diff --git a/src/drivers/dynamixel/dynamixel_sdk.h b/src/drivers/dynamixel/dynamixel_sdk.h new file mode 100644 index 0000000..c191698 --- /dev/null +++ b/src/drivers/dynamixel/dynamixel_sdk.h @@ -0,0 +1,34 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file that includes whole Dynamixel SDK libraries +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ + +#include "group_bulk_read.h" +#include "group_bulk_write.h" +#include "group_fast_bulk_read.h" +#include "group_fast_sync_read.h" +#include "group_sync_read.h" +#include "group_sync_write.h" +#include "packet_handler.h" +#include "port_handler.h" + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/group_bulk_read.cpp b/src/drivers/dynamixel/group_bulk_read.cpp new file mode 100644 index 0000000..aa58227 --- /dev/null +++ b/src/drivers/dynamixel/group_bulk_read.cpp @@ -0,0 +1,223 @@ +/******************************************************************************* + * 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: zerom, Ryu Woon Jung (Leon) */ + +#include +#include + +#if defined(__linux__) +#include "group_bulk_read.h" +#elif defined(__APPLE__) +#include "group_bulk_read.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "group_bulk_read.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include "../../include/dynamixel_sdk/group_bulk_read.h" +#endif + +using namespace dynamixel; + +GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) + : GroupHandler(port, ph), last_result_(false) { + clearParam(); +} + +void GroupBulkRead::makeParam() { + if (id_list_.size() == 0) + return; + + if (param_ != 0) + delete[] param_; + param_ = 0; + + if (ph_->getProtocolVersion() == 1.0) { + param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) + } else // 2.0 + { + param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) + } + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) { + uint8_t id = id_list_[i]; + if (ph_->getProtocolVersion() == 1.0) { + param_[idx++] = (uint8_t)length_list_[id]; // LEN + param_[idx++] = id; // ID + param_[idx++] = (uint8_t)address_list_[id]; // ADDR + } else // 2.0 + { + param_[idx++] = id; // ID + param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L + param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H + param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L + param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H + } + } +} + +bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, + uint16_t data_length) { + if (std::find(id_list_.begin(), id_list_.end(), id) != + id_list_.end()) // id already exist + return false; + + id_list_.push_back(id); + 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; +} + +void GroupBulkRead::removeParam(uint8_t id) { + std::vector::iterator it = + std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; + + id_list_.erase(it); + 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; +} + +void GroupBulkRead::clearParam() { + if (id_list_.size() == 0) + 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; +} + +int GroupBulkRead::txPacket() { + if (id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + if (is_param_changed_ == true || param_ == 0) + makeParam(); + + if (ph_->getProtocolVersion() == 1.0) { + return ph_->bulkReadTx(port_, param_, id_list_.size() * 3); + } else // 2.0 + { + return ph_->bulkReadTx(port_, param_, id_list_.size() * 5); + } +} + +int GroupBulkRead::rxPacket() { + int cnt = id_list_.size(); + int result = COMM_RX_FAIL; + + last_result_ = false; + + if (cnt == 0) + return COMM_NOT_AVAILABLE; + + for (int i = 0; i < cnt; i++) { + uint8_t id = id_list_[i]; + + result = ph_->readRx(port_, id, length_list_[id], data_list_[id], + error_list_[id]); + if (result != COMM_SUCCESS) + return result; + } + + if (result == COMM_SUCCESS) + last_result_ = true; + + return result; +} + +int GroupBulkRead::txRxPacket() { + int result = COMM_TX_FAIL; + + result = txPacket(); + if (result != COMM_SUCCESS) + return result; + + return rxPacket(); +} + +bool GroupBulkRead::isAvailable(uint8_t id, uint16_t address, + uint16_t data_length) { + uint16_t start_addr; + + if (last_result_ == false || data_list_.find(id) == data_list_.end()) + return false; + + start_addr = address_list_[id]; + + if (address < start_addr || + start_addr + length_list_[id] - data_length < address) + return false; + + return true; +} + +uint32_t GroupBulkRead::getData(uint8_t id, uint16_t address, + uint16_t data_length) { + if (isAvailable(id, address, data_length) == false) + return 0; + + uint16_t start_addr = address_list_[id]; + + switch (data_length) { + case 1: + return data_list_[id][address - start_addr]; + + case 2: + return DXL_MAKEWORD(data_list_[id][address - start_addr], + data_list_[id][address - start_addr + 1]); + + case 4: + return DXL_MAKEDWORD( + DXL_MAKEWORD(data_list_[id][address - start_addr + 0], + data_list_[id][address - start_addr + 1]), + DXL_MAKEWORD(data_list_[id][address - start_addr + 2], + data_list_[id][address - start_addr + 3])); + + default: + 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()) + + return error[0] = error_list_[id][0]; +} \ No newline at end of file diff --git a/src/drivers/dynamixel/group_bulk_read.h b/src/drivers/dynamixel/group_bulk_read.h new file mode 100644 index 0000000..055801d --- /dev/null +++ b/src/drivers/dynamixel/group_bulk_read.h @@ -0,0 +1,153 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Dynamixel Bulk Read +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ + +#include "group_handler.h" +#include "packet_handler.h" +#include "port_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for reading multiple Dynamixel data from different +/// addresses with different lengths at once +//////////////////////////////////////////////////////////////////////////////// +class WINDECLSPEC GroupBulkRead : public GroupHandler { +protected: + std::map address_list_; // + std::map length_list_; // + std::map error_list_; // + + bool last_result_; + + void makeParam(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that Initializes instance for Bulk Read + /// @param port PortHandler instance + /// @param ph PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + GroupBulkRead(PortHandler *port, PacketHandler *ph); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls clearParam function to clear the parameter + /// list for Bulk Read + //////////////////////////////////////////////////////////////////////////////// + ~GroupBulkRead() { clearParam(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that adds id, start_address, data_length to the Bulk + /// Read list + /// @param id Dynamixel ID + /// @param start_address Address of the data for read + /// @data_length Length of the data for read + /// @return false + /// @return when the ID exists already in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool addParam(uint8_t id, uint16_t start_address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that removes id from the Bulk Read list + /// @param id Dynamixel ID + //////////////////////////////////////////////////////////////////////////////// + void removeParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the Bulk Read list + //////////////////////////////////////////////////////////////////////////////// + void clearParam(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the Bulk Read instruction packet which + /// might be constructed by GroupBulkRead::addParam function + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Bulk Read is empty + /// @return or the other communication results which come from + /// PacketHandler::bulkReadTx + //////////////////////////////////////////////////////////////////////////////// + int txPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet which might be come from the + /// Dynamixel + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Bulk Read is empty + /// @return COMM_RX_FAIL + /// @return when there is no packet recieved + /// @return COMM_SUCCESS + /// @return when there is packet recieved + /// @return or the other communication results + //////////////////////////////////////////////////////////////////////////////// + int rxPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits and receives the packet which might be + /// come from the Dynamixel + /// @return COMM_RX_FAIL + /// @return when there is no packet recieved + /// @return COMM_SUCCESS + /// @return when there is packet recieved + /// @return or the other communication results which come from + /// GroupBulkRead::txPacket or GroupBulkRead::rxPacket + //////////////////////////////////////////////////////////////////////////////// + int txRxPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether there are available data which + /// might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param data_length Length of the data for read + /// @return false + /// @return when there are no data available + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool isAvailable(uint8_t id, uint16_t address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets the data which might be received by + /// GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @data_length Length of the data for read + /// @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); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/group_bulk_write.cpp b/src/drivers/dynamixel/group_bulk_write.cpp new file mode 100644 index 0000000..9fc62f8 --- /dev/null +++ b/src/drivers/dynamixel/group_bulk_write.cpp @@ -0,0 +1,149 @@ +/******************************************************************************* + * 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: zerom, Ryu Woon Jung (Leon) */ + +#include + +#if defined(__linux__) +#include "group_bulk_write.h" +#elif defined(__APPLE__) +#include "group_bulk_write.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "group_bulk_write.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include "../../include/dynamixel_sdk/group_bulk_write.h" +#endif + +using namespace dynamixel; + +GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) + : GroupHandler(port, ph), param_length_(0) { + clearParam(); +} + +void GroupBulkWrite::makeParam() { + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + if (param_ != 0) + delete[] param_; + param_ = 0; + + param_length_ = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]]; + + param_ = new uint8_t[param_length_]; + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) { + uint8_t id = id_list_[i]; + if (data_list_[id] == 0) + return; + + param_[idx++] = id; + param_[idx++] = DXL_LOBYTE(address_list_[id]); + param_[idx++] = DXL_HIBYTE(address_list_[id]); + param_[idx++] = DXL_LOBYTE(length_list_[id]); + param_[idx++] = DXL_HIBYTE(length_list_[id]); + for (int c = 0; c < length_list_[id]; c++) + param_[idx++] = (data_list_[id])[c]; + } +} + +bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, + uint16_t data_length, uint8_t *data) { + if (ph_->getProtocolVersion() == 1.0) + return false; + + if (std::find(id_list_.begin(), id_list_.end(), id) != + id_list_.end()) // id already exist + return false; + + id_list_.push_back(id); + address_list_[id] = start_address; + length_list_[id] = data_length; + data_list_[id] = new uint8_t[data_length]; + for (int c = 0; c < data_length; c++) + data_list_[id][c] = data[c]; + + is_param_changed_ = true; + return true; +} +void GroupBulkWrite::removeParam(uint8_t id) { + if (ph_->getProtocolVersion() == 1.0) + return; + + std::vector::iterator it = + std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; + + id_list_.erase(it); + address_list_.erase(id); + length_list_.erase(id); + delete[] data_list_[id]; + data_list_.erase(id); + + is_param_changed_ = true; +} +bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, + uint16_t data_length, uint8_t *data) { + if (ph_->getProtocolVersion() == 1.0) + return false; + + std::vector::iterator it = + std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return false; + + address_list_[id] = start_address; + length_list_[id] = data_length; + delete[] data_list_[id]; + data_list_[id] = new uint8_t[data_length]; + for (int c = 0; c < data_length; c++) + data_list_[id][c] = data[c]; + + is_param_changed_ = true; + return true; +} +void GroupBulkWrite::clearParam() { + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; + + id_list_.clear(); + address_list_.clear(); + length_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; +} +int GroupBulkWrite::txPacket() { + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + if (is_param_changed_ == true || param_ == 0) + makeParam(); + + return ph_->bulkWriteTxOnly(port_, param_, param_length_); +} \ No newline at end of file diff --git a/src/drivers/dynamixel/group_bulk_write.h b/src/drivers/dynamixel/group_bulk_write.h new file mode 100644 index 0000000..a2e1207 --- /dev/null +++ b/src/drivers/dynamixel/group_bulk_write.h @@ -0,0 +1,110 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Dynamixel Bulk Write +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ + +#include "group_handler.h" +#include "packet_handler.h" +#include "port_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for writing multiple Dynamixel data from different +/// addresses with different lengths at once +//////////////////////////////////////////////////////////////////////////////// +class WINDECLSPEC GroupBulkWrite : public GroupHandler { +private: + std::map address_list_; // + std::map length_list_; // + + uint16_t param_length_; + + void makeParam(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that Initializes instance for Bulk Write + /// @param port PortHandler instance + /// @param ph PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + GroupBulkWrite(PortHandler *port, PacketHandler *ph); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls clearParam function to clear the parameter + /// list for Bulk Write + //////////////////////////////////////////////////////////////////////////////// + ~GroupBulkWrite() { clearParam(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that adds id, start_address, data_length to the Bulk + /// Write list + /// @param id Dynamixel ID + /// @param start_address Address of the data for write + /// @param data_length Length of the data for write + /// @return false + /// @return when the ID exists already in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool addParam(uint8_t id, uint16_t start_address, uint16_t data_length, + uint8_t *data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that removes id from the Bulk Write list + /// @param id Dynamixel ID + //////////////////////////////////////////////////////////////////////////////// + void removeParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that changes the data for write in id -> start_address + /// -> data_length to the Bulk Write list + /// @param id Dynamixel ID + /// @param start_address Address of the data for write + /// @param data_length Length of the data for write + /// @param data for replacement + /// @return false + /// @return when the ID doesn't exist in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, + uint8_t *data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the Bulk Write list + //////////////////////////////////////////////////////////////////////////////// + void clearParam(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the Bulk Write instruction packet which + /// might be constructed by GroupBulkWrite::addParam function + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Bulk Write is empty + /// @return when Protocol1.0 has been used + /// @return or the other communication results which come from + /// PacketHandler::bulkWriteTxOnly + //////////////////////////////////////////////////////////////////////////////// + int txPacket(); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/group_fast_bulk_read.cpp b/src/drivers/dynamixel/group_fast_bulk_read.cpp new file mode 100644 index 0000000..0ccdf9e --- /dev/null +++ b/src/drivers/dynamixel/group_fast_bulk_read.cpp @@ -0,0 +1,116 @@ +/******************************************************************************* + * 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: Honghyun Kim */ + +#include + +#if defined(__linux__) +#include "group_fast_bulk_read.h" +#elif defined(__APPLE__) +#include "group_fast_bulk_read.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "group_fast_bulk_read.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) +#include "../../include/dynamixel_sdk/group_fast_bulk_read.h" +#endif + +const int RXPACKET_MAX_LEN = 1024; +const int PKT_ID = 4; +const int PKT_PARAMETER0 = 8; + +using namespace dynamixel; + +GroupFastBulkRead::GroupFastBulkRead(PortHandler *port, PacketHandler *ph) + : GroupBulkRead(port, ph) { + clearParam(); +} + +void GroupFastBulkRead::makeParam() { + if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) + return; + + if (0 != param_) + delete[] param_; + param_ = 0; + + param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) { + uint8_t id = id_list_[i]; + param_[idx++] = id; // ID + param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L + param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H + param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L + param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H + } +} + +int GroupFastBulkRead::txPacket() { + if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) + return COMM_NOT_AVAILABLE; + + if ((true == is_param_changed_) || (0 == param_)) + makeParam(); + + return ph_->fastBulkReadTx(port_, param_, id_list_.size() * 5); +} + +int GroupFastBulkRead::rxPacket() { + last_result_ = false; + + if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) + return COMM_NOT_AVAILABLE; + + int count = id_list_.size(); + int result = COMM_RX_FAIL; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + if (NULL == rxpacket) + return result; + + do { + result = ph_->rxPacket(port_, rxpacket, true); + } while ((COMM_SUCCESS == result) && (BROADCAST_ID != rxpacket[PKT_ID])); + + if ((COMM_SUCCESS == result) && (BROADCAST_ID == rxpacket[PKT_ID])) { + int index = PKT_PARAMETER0; + for (int i = 0; i < count; ++i) { + uint8_t id = id_list_[i]; + uint16_t length = length_list_[id]; + *error_list_[id] = (uint8_t)rxpacket[index]; + for (uint16_t s = 0; s < length; s++) { + data_list_[id][s] = rxpacket[index + 2 + s]; + } + index += (length + 4); + } + last_result_ = true; + } + + free(rxpacket); + return result; +} + +int GroupFastBulkRead::txRxPacket() { + if (1.0 == ph_->getProtocolVersion()) + return COMM_NOT_AVAILABLE; + + int result = txPacket(); + if (COMM_SUCCESS != result) + return result; + return rxPacket(); +} \ No newline at end of file diff --git a/src/drivers/dynamixel/group_fast_bulk_read.h b/src/drivers/dynamixel/group_fast_bulk_read.h new file mode 100644 index 0000000..6d80cc7 --- /dev/null +++ b/src/drivers/dynamixel/group_fast_bulk_read.h @@ -0,0 +1,44 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Dynamixel Fast Bulk Read +/// @author Honghyun Kim +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPFASTBULKREAD_H +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPFASTBULKREAD_H + +#include "group_bulk_read.h" + +namespace dynamixel { + +class WINDECLSPEC GroupFastBulkRead : public GroupBulkRead { +public: + GroupFastBulkRead(PortHandler *port, PacketHandler *ph); + ~GroupFastBulkRead() { clearParam(); } + + int txPacket(); + int rxPacket(); + int txRxPacket(); + +private: + void makeParam(); +}; + +} // namespace dynamixel + +#endif // DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPFASTBULKREAD_H \ No newline at end of file diff --git a/src/drivers/dynamixel/group_fast_sync_read.cpp b/src/drivers/dynamixel/group_fast_sync_read.cpp new file mode 100644 index 0000000..fd76ea0 --- /dev/null +++ b/src/drivers/dynamixel/group_fast_sync_read.cpp @@ -0,0 +1,97 @@ +/******************************************************************************* + * 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: Honghyun Kim */ + +#include + +#if defined(__linux__) +#include "group_fast_sync_read.h" +#elif defined(__APPLE__) +#include "group_fast_sync_read.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "group_fast_sync_read.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) +#include "../../include/dynamixel_sdk/group_fast_sync_read.h" +#endif + +const int RXPACKET_MAX_LEN = 1024; +const int PKT_ID = 4; +const int PKT_PARAMETER0 = 8; + +using namespace dynamixel; + +GroupFastSyncRead::GroupFastSyncRead(PortHandler *port, PacketHandler *ph, + uint16_t start_address, + uint16_t data_length) + : GroupSyncRead(port, ph, start_address, data_length) { + clearParam(); +} + +int GroupFastSyncRead::txPacket() { + if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) + return COMM_NOT_AVAILABLE; + + if ((true == is_param_changed_) || (0 == param_)) + makeParam(); + + return ph_->fastSyncReadTx(port_, start_address_, data_length_, param_, + (uint16_t)id_list_.size() * 1); +} + +int GroupFastSyncRead::rxPacket() { + last_result_ = false; + + if ((1.0 == ph_->getProtocolVersion()) || (id_list_.empty())) + return COMM_NOT_AVAILABLE; + + int count = id_list_.size(); + int result = COMM_RX_FAIL; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + if (NULL == rxpacket) + return result; + + do { + result = ph_->rxPacket(port_, rxpacket, true); + } while ((COMM_SUCCESS == result) && (BROADCAST_ID != rxpacket[PKT_ID])); + + if ((COMM_SUCCESS == result) && (BROADCAST_ID == rxpacket[PKT_ID])) { + int index = PKT_PARAMETER0; + for (int i = 0; i < count; ++i) { + uint8_t id = id_list_[i]; + *error_list_[id] = (uint8_t)rxpacket[index]; + for (uint16_t s = 0; s < data_length_; s++) { + data_list_[id][s] = rxpacket[index + 2 + s]; + } + index += (data_length_ + 4); + } + last_result_ = true; + } + + free(rxpacket); + return result; +} + +int GroupFastSyncRead::txRxPacket() { + if (1.0 == ph_->getProtocolVersion()) + return COMM_NOT_AVAILABLE; + + int result = txPacket(); + if (COMM_SUCCESS != result) + return result; + return rxPacket(); +} \ No newline at end of file diff --git a/src/drivers/dynamixel/group_fast_sync_read.h b/src/drivers/dynamixel/group_fast_sync_read.h new file mode 100644 index 0000000..d549dfa --- /dev/null +++ b/src/drivers/dynamixel/group_fast_sync_read.h @@ -0,0 +1,42 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Dynamixel Fast Sync Read +/// @author Honghyun Kim +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPFASTSYNCREAD_H +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPFASTSYNCREAD_H + +#include "group_sync_read.h" + +namespace dynamixel { + +class WINDECLSPEC GroupFastSyncRead : public GroupSyncRead { +public: + GroupFastSyncRead(PortHandler *port, PacketHandler *ph, + uint16_t start_address, uint16_t data_length); + ~GroupFastSyncRead() { clearParam(); } + + int txPacket(); + int rxPacket(); + int txRxPacket(); +}; + +} // namespace dynamixel + +#endif // DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPFASTSYNCREAD_H \ No newline at end of file diff --git a/src/drivers/dynamixel/group_handler.cpp b/src/drivers/dynamixel/group_handler.cpp new file mode 100644 index 0000000..6a1aba0 --- /dev/null +++ b/src/drivers/dynamixel/group_handler.cpp @@ -0,0 +1,33 @@ +/******************************************************************************* + * 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: Honghyun Kim */ + +#if defined(__linux__) +#include "group_handler.h" +#elif defined(__APPLE__) +#include "group_handler.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "group_handler.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) +#include "../../include/dynamixel_sdk/group_handler.h" +#endif + +using namespace dynamixel; + +GroupHandler::GroupHandler(PortHandler *port, PacketHandler *ph) + : port_(port), ph_(ph), is_param_changed_(false), param_(0) {} \ No newline at end of file diff --git a/src/drivers/dynamixel/group_handler.h b/src/drivers/dynamixel/group_handler.h new file mode 100644 index 0000000..cb4b411 --- /dev/null +++ b/src/drivers/dynamixel/group_handler.h @@ -0,0 +1,53 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Group Handler +/// @author Honghyun Kim +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPHANDLER_H +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPHANDLER_H + +#include "packet_handler.h" +#include "port_handler.h" +#include +#include + +namespace dynamixel { + +class WINDECLSPEC GroupHandler { +public: + GroupHandler(PortHandler *port, PacketHandler *ph); + + PortHandler *getPortHandler() { return port_; } + PacketHandler *getPacketHandler() { return ph_; } + +protected: + PortHandler *port_; + PacketHandler *ph_; + + std::vector id_list_; + std::map data_list_; // + + bool is_param_changed_; + + uint8_t *param_; +}; + +} // namespace dynamixel + +#endif // DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPHANDLER_H \ No newline at end of file diff --git a/src/drivers/dynamixel/group_sync_read.cpp b/src/drivers/dynamixel/group_sync_read.cpp new file mode 100644 index 0000000..4358a5d --- /dev/null +++ b/src/drivers/dynamixel/group_sync_read.cpp @@ -0,0 +1,201 @@ +/******************************************************************************* + * 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: zerom, Ryu Woon Jung (Leon) */ + +#include + +#if defined(__linux__) +#include "group_sync_read.h" +#elif defined(__APPLE__) +#include "group_sync_read.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "group_sync_read.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include "../../include/dynamixel_sdk/group_sync_read.h" +#endif + +using namespace dynamixel; + +GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, + uint16_t start_address, uint16_t data_length) + : GroupHandler(port, ph), last_result_(false), + start_address_(start_address), data_length_(data_length) { + clearParam(); +} + +void GroupSyncRead::makeParam() { + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + if (param_ != 0) + delete[] param_; + param_ = 0; + + param_ = new uint8_t[id_list_.size() * 1]; // ID(1) + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + param_[idx++] = id_list_[i]; +} + +bool GroupSyncRead::addParam(uint8_t id) { + if (ph_->getProtocolVersion() == 1.0) + return false; + + if (std::find(id_list_.begin(), id_list_.end(), id) != + id_list_.end()) // id already exist + return false; + + 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; +} +void GroupSyncRead::removeParam(uint8_t id) { + if (ph_->getProtocolVersion() == 1.0) + return; + + std::vector::iterator it = + std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; + + id_list_.erase(it); + delete[] data_list_[id]; + delete[] error_list_[id]; + data_list_.erase(id); + error_list_.erase(id); + + is_param_changed_ = true; +} +void GroupSyncRead::clearParam() { + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + 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; +} + +int GroupSyncRead::txPacket() { + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + if (is_param_changed_ == true || param_ == 0) + makeParam(); + + return ph_->syncReadTx(port_, start_address_, data_length_, param_, + (uint16_t)id_list_.size() * 1); +} + +int GroupSyncRead::rxPacket() { + last_result_ = false; + + if (ph_->getProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; + + int cnt = id_list_.size(); + int result = COMM_RX_FAIL; + + if (cnt == 0) + return COMM_NOT_AVAILABLE; + + for (int i = 0; i < cnt; i++) { + uint8_t id = id_list_[i]; + + result = + ph_->readRx(port_, id, data_length_, data_list_[id], error_list_[id]); + if (result != COMM_SUCCESS) + return result; + } + + if (result == COMM_SUCCESS) + last_result_ = true; + + return result; +} + +int GroupSyncRead::txRxPacket() { + if (ph_->getProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; + + int result = COMM_TX_FAIL; + + result = txPacket(); + if (result != COMM_SUCCESS) + return result; + + return rxPacket(); +} + +bool GroupSyncRead::isAvailable(uint8_t id, uint16_t address, + uint16_t data_length) { + if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || + data_list_.find(id) == data_list_.end()) + return false; + + if (address < start_address_ || + start_address_ + data_length_ - data_length < address) + return false; + + return true; +} + +uint32_t GroupSyncRead::getData(uint8_t id, uint16_t address, + uint16_t data_length) { + if (isAvailable(id, address, data_length) == false) + return 0; + + switch (data_length) { + case 1: + return data_list_[id][address - start_address_]; + + case 2: + return DXL_MAKEWORD(data_list_[id][address - start_address_], + data_list_[id][address - start_address_ + 1]); + + case 4: + return DXL_MAKEDWORD( + DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], + data_list_[id][address - start_address_ + 1]), + DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], + data_list_[id][address - start_address_ + 3])); + + default: + 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()) + + return error[0] = error_list_[id][0]; +} \ No newline at end of file diff --git a/src/drivers/dynamixel/group_sync_read.h b/src/drivers/dynamixel/group_sync_read.h new file mode 100644 index 0000000..cc31c6a --- /dev/null +++ b/src/drivers/dynamixel/group_sync_read.h @@ -0,0 +1,159 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Dynamixel Sync Read +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ + +#include "group_handler.h" +#include "packet_handler.h" +#include "port_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for reading multiple Dynamixel data from same address with +/// same length at once +//////////////////////////////////////////////////////////////////////////////// +class WINDECLSPEC GroupSyncRead : public GroupHandler { +protected: + std::map error_list_; // + + bool last_result_; + + uint16_t start_address_; + uint16_t data_length_; + + void makeParam(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that Initializes instance for Sync Read + /// @param port PortHandler instance + /// @param ph PacketHandler instance + /// @param start_address Address of the data for read + /// @param data_length Length of the data for read + //////////////////////////////////////////////////////////////////////////////// + GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, + uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls clearParam function to clear the parameter + /// list for Sync Read + //////////////////////////////////////////////////////////////////////////////// + ~GroupSyncRead() { clearParam(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that adds id, start_address, data_length to the Sync + /// Read list + /// @param id Dynamixel ID + /// @return false + /// @return when the ID exists already in the list + /// @return when the protocol1.0 has been used + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool addParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that removes id from the Sync Read list + /// @param id Dynamixel ID + //////////////////////////////////////////////////////////////////////////////// + void removeParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the Sync Read list + //////////////////////////////////////////////////////////////////////////////// + void clearParam(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the Sync Read instruction packet which + /// might be constructed by GroupSyncRead::addParam function + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Sync Read is empty + /// @return when the protocol1.0 has been used + /// @return or the other communication results which come from + /// PacketHandler::syncReadTx + //////////////////////////////////////////////////////////////////////////////// + int txPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet which might be come from the + /// Dynamixel + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Sync Read is empty + /// @return when the protocol1.0 has been used + /// @return COMM_SUCCESS + /// @return when there is packet recieved + /// @return or the other communication results + //////////////////////////////////////////////////////////////////////////////// + int rxPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits and receives the packet which might be + /// come from the Dynamixel + /// @return COMM_NOT_AVAILABLE + /// @return when the protocol1.0 has been used + /// @return COMM_RX_FAIL + /// @return when there is no packet recieved + /// @return COMM_SUCCESS + /// @return when there is packet recieved + /// @return or the other communication results which come from + /// GroupBulkRead::txPacket or GroupBulkRead::rxPacket + //////////////////////////////////////////////////////////////////////////////// + int txRxPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether there are available data which + /// might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param data_length Length of the data for read + /// @return false + /// @return when there are no data available + /// @return when the protocol1.0 has been used + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool isAvailable(uint8_t id, uint16_t address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets the data which might be received by + /// GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @data_length Length of the data for read + /// @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); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/group_sync_write.cpp b/src/drivers/dynamixel/group_sync_write.cpp new file mode 100644 index 0000000..264d0b8 --- /dev/null +++ b/src/drivers/dynamixel/group_sync_write.cpp @@ -0,0 +1,130 @@ +/******************************************************************************* + * 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: zerom, Ryu Woon Jung (Leon) */ + +#include + +#if defined(__linux__) +#include "group_sync_write.h" +#elif defined(__APPLE__) +#include "group_sync_write.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "group_sync_write.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include "../../include/dynamixel_sdk/group_sync_write.h" +#endif + +using namespace dynamixel; + +GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, + uint16_t start_address, uint16_t data_length) + : GroupHandler(port, ph), start_address_(start_address), + data_length_(data_length) { + clearParam(); +} + +void GroupSyncWrite::makeParam() { + if (id_list_.size() == 0) + return; + + if (param_ != 0) + delete[] param_; + param_ = 0; + + param_ = new uint8_t[id_list_.size() * + (1 + data_length_)]; // ID(1) + DATA(data_length) + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) { + uint8_t id = id_list_[i]; + if (data_list_[id] == 0) + return; + + param_[idx++] = id; + for (int c = 0; c < data_length_; c++) + param_[idx++] = (data_list_[id])[c]; + } +} + +bool GroupSyncWrite::addParam(uint8_t id, uint8_t *data) { + if (std::find(id_list_.begin(), id_list_.end(), id) != + id_list_.end()) // id already exist + return false; + + id_list_.push_back(id); + data_list_[id] = new uint8_t[data_length_]; + for (int c = 0; c < data_length_; c++) + data_list_[id][c] = data[c]; + + is_param_changed_ = true; + return true; +} + +void GroupSyncWrite::removeParam(uint8_t id) { + std::vector::iterator it = + std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; + + id_list_.erase(it); + delete[] data_list_[id]; + data_list_.erase(id); + + is_param_changed_ = true; +} + +bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) { + std::vector::iterator it = + std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return false; + + delete[] data_list_[id]; + data_list_[id] = new uint8_t[data_length_]; + for (int c = 0; c < data_length_; c++) + data_list_[id][c] = data[c]; + + is_param_changed_ = true; + return true; +} + +void GroupSyncWrite::clearParam() { + if (id_list_.size() == 0) + return; + + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; + + id_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; +} + +int GroupSyncWrite::txPacket() { + if (id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + if (is_param_changed_ == true || param_ == 0) + makeParam(); + + return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, + id_list_.size() * (1 + data_length_)); +} \ No newline at end of file diff --git a/src/drivers/dynamixel/group_sync_write.h b/src/drivers/dynamixel/group_sync_write.h new file mode 100644 index 0000000..1015823 --- /dev/null +++ b/src/drivers/dynamixel/group_sync_write.h @@ -0,0 +1,105 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Dynamixel Sync Write +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ + +#include "group_handler.h" +#include "packet_handler.h" +#include "port_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for writing multiple Dynamixel data from same address with +/// same length at once +//////////////////////////////////////////////////////////////////////////////// +class WINDECLSPEC GroupSyncWrite : public GroupHandler { +private: + uint16_t start_address_; + uint16_t data_length_; + + void makeParam(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that Initializes instance for Sync Write + /// @param port PortHandler instance + /// @param ph PacketHandler instance + /// @param start_address Address of the data for write + /// @param data_length Length of the data for write + //////////////////////////////////////////////////////////////////////////////// + GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, + uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls clearParam function to clear the parameter + /// list for Sync Write + //////////////////////////////////////////////////////////////////////////////// + ~GroupSyncWrite() { clearParam(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that adds id, start_address, data_length to the Sync + /// Write list + /// @param id Dynamixel ID + /// @param data Data for write + /// @return false + /// @return when the ID exists already in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool addParam(uint8_t id, uint8_t *data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that removes id from the Sync Write list + /// @param id Dynamixel ID + //////////////////////////////////////////////////////////////////////////////// + void removeParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that changes the data for write in id -> start_address + /// -> data_length to the Sync Write list + /// @param id Dynamixel ID + /// @param data for replacement + /// @return false + /// @return when the ID doesn't exist in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool changeParam(uint8_t id, uint8_t *data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the Sync Write list + //////////////////////////////////////////////////////////////////////////////// + void clearParam(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the Sync Write instruction packet which + /// might be constructed by GroupSyncWrite::addParam function + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Sync Write is empty + /// @return or the other communication results which come from + /// PacketHandler::syncWriteTxOnly + //////////////////////////////////////////////////////////////////////////////// + int txPacket(); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/packet_handler.cpp b/src/drivers/dynamixel/packet_handler.cpp new file mode 100644 index 0000000..dd7733c --- /dev/null +++ b/src/drivers/dynamixel/packet_handler.cpp @@ -0,0 +1,49 @@ +/******************************************************************************* + * 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: zerom, Ryu Woon Jung (Leon) */ + +#if defined(__linux__) +#include "packet_handler.h" +#include "protocol1_packet_handler.h" +#include "protocol2_packet_handler.h" +#elif defined(__APPLE__) +#include "packet_handler.h" +#include "protocol1_packet_handler.h" +#include "protocol2_packet_handler.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "packet_handler.h" +#include "protocol1_packet_handler.h" +#include "protocol2_packet_handler.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include "../../include/dynamixel_sdk/packet_handler.h" +#include "../../include/dynamixel_sdk/protocol1_packet_handler.h" +#include "../../include/dynamixel_sdk/protocol2_packet_handler.h" +#endif + +using namespace dynamixel; + +PacketHandler *PacketHandler::getPacketHandler(float protocol_version) { + if (protocol_version == 1.0) { + return (PacketHandler *)(Protocol1PacketHandler::getInstance()); + } else if (protocol_version == 2.0) { + return (PacketHandler *)(Protocol2PacketHandler::getInstance()); + } + + return (PacketHandler *)(Protocol2PacketHandler::getInstance()); +} \ No newline at end of file diff --git a/src/drivers/dynamixel/packet_handler.h b/src/drivers/dynamixel/packet_handler.h new file mode 100644 index 0000000..0cfa68b --- /dev/null +++ b/src/drivers/dynamixel/packet_handler.h @@ -0,0 +1,710 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Dynamixel packet control +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ + +#if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include + +#define ERROR_PRINT SerialBT2.print +#else +#define ERROR_PRINT printf + +#endif + +#include "port_handler.h" +#include +#include + +#define BROADCAST_ID 0xFE // 254 +#define MAX_ID 0xFC // 252 + +/* Macro for Control Table Value */ +#define DXL_MAKEWORD(a, b) \ + ((uint16_t)(((uint8_t)(((uint64_t)(a)) & 0xff)) | \ + ((uint16_t)((uint8_t)(((uint64_t)(b)) & 0xff))) << 8)) +#define DXL_MAKEDWORD(a, b) \ + ((uint32_t)(((uint16_t)(((uint64_t)(a)) & 0xffff)) | \ + ((uint32_t)((uint16_t)(((uint64_t)(b)) & 0xffff))) << 16)) +#define DXL_LOWORD(l) ((uint16_t)(((uint64_t)(l)) & 0xffff)) +#define DXL_HIWORD(l) ((uint16_t)((((uint64_t)(l)) >> 16) & 0xffff)) +#define DXL_LOBYTE(w) ((uint8_t)(((uint64_t)(w)) & 0xff)) +#define DXL_HIBYTE(w) ((uint8_t)((((uint64_t)(w)) >> 8) & 0xff)) + +/* Instruction for DXL Protocol */ +#define INST_PING 1 +#define INST_READ 2 +#define INST_WRITE 3 +#define INST_REG_WRITE 4 +#define INST_ACTION 5 +#define INST_FACTORY_RESET 6 +#define INST_SYNC_WRITE 131 // 0x83 +#define INST_BULK_READ 146 // 0x92 +// --- Only for 2.0 --- // +#define INST_REBOOT 8 +#define INST_CLEAR 16 // 0x10 +#define INST_STATUS 85 // 0x55 +#define INST_SYNC_READ 130 // 0x82 +#define INST_BULK_WRITE 147 // 0x93 +// Fast +const uint8_t INST_FAST_SYNC_READ = 0x8A; +const uint8_t INST_FAST_BULK_READ = 0x9A; + +// Communication Result +#define COMM_SUCCESS 0 // tx or rx packet communication success +#define COMM_PORT_BUSY (-1000) // Port is busy (in use) +#define COMM_TX_FAIL (-1001) // Failed transmit instruction packet +#define COMM_RX_FAIL (-1002) // Failed get status packet +#define COMM_TX_ERROR (-2000) // Incorrect instruction packet +#define COMM_RX_WAITING (-3000) // Now recieving status packet +#define COMM_RX_TIMEOUT (-3001) // There is no status packet +#define COMM_RX_CORRUPT (-3002) // Incorrect status packet +#define COMM_NOT_AVAILABLE (-9000) // + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class that inherits Protocol1PacketHandler class or +/// Protocol2PacketHandler class +//////////////////////////////////////////////////////////////////////////////// +class WINDECLSPEC PacketHandler { +protected: + PacketHandler() {} + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PacketHandler instance + /// @return PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + static PacketHandler *getPacketHandler(float protocol_version = 2.0); + + virtual ~PacketHandler() {} + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol version + /// @return protocol version + //////////////////////////////////////////////////////////////////////////////// + virtual float getProtocolVersion() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets description of communication result + /// @param result Communication result which might be gotten by the tx rx + /// functions + /// @return description of communication result in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + virtual const char *getTxRxResult(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 + /// @return description of hardware error in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + virtual const char *getRxPacketError(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, + /// @description then transmits txpacket by PortHandler::writePort() + /// function. + /// @description The function activates only when the port is not busy and + /// when the packet is already written on the port buffer + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @return COMM_PORT_BUSY + /// @return when the port is already in use + /// @return COMM_TX_ERROR + /// @return when txpacket is out of range described by TXPACKET_MAX_LEN + /// @return COMM_TX_FAIL + /// @return when written packet is shorter than expected + /// @return or COMM_SUCCESS + //////////////////////////////////////////////////////////////////////////////// + virtual int txPacket(PortHandler *port, uint8_t *txpacket) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives packet (rxpacket) during designated time + /// via PortHandler port + /// @description The function repeatedly tries to receive rxpacket by + /// PortHandler::readPort() function. + /// @description It breaks out + /// @description when PortHandler::isPacketTimeout() shows the timeout, + /// @description when rxpacket seemed as corrupted, or + /// @description when nothing received + /// @param port PortHandler instance + /// @param rxpacket received packet + /// @return COMM_RX_CORRUPT + /// @return when it received the packet but it couldn't find header in the + /// packet + /// @return when it found header in the packet but the id, length or error + /// value is out of range + /// @return when it received the packet but it is shorted than expected + /// @return COMM_RX_TIMEOUT + /// @return when there is no rxpacket received until + /// PortHandler::isPacketTimeout() shows the timeout + /// @return COMM_SUCCESS + /// @return when rxpacket passes checksum test + /// @return or COMM_RX_FAIL + //////////////////////////////////////////////////////////////////////////////// + virtual int rxPacket(PortHandler *port, uint8_t *rxpacket, + bool skip_stuffing = false) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits packet (txpacket) and receives packet + /// (rxpacket) during designated time via PortHandler port + /// @description The function calls PacketHandler::txPacket(), + /// @description and calls PacketHandler::rxPacket() if it succeeds + /// PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it fails PacketHandler::txPacket(), + /// @description when txpacket is called by PacketHandler::broadcastPing() / + /// PacketHandler::syncWriteTxOnly() / PacketHandler::regWriteTxOnly / + /// PacketHandler::action + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @param rxpacket received packet + /// @return COMM_SUCCESS + /// @return when it succeeds PacketHandler::txPacket() and + /// PacketHandler::rxPacket() + /// @return or the other communication results which come from + /// PacketHandler::txPacket() and PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int txRxPacket(PortHandler *port, uint8_t *txpacket, + uint8_t *rxpacket, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel but doesn't take its model number + /// @description The function calls PacketHandler::ping() which gets Dynamixel + /// model number, + /// @description but doesn't carry the model number + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::ping() + //////////////////////////////////////////////////////////////////////////////// + virtual int ping(PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel and takes its model number + /// @description The function makes an instruction packet with INST_PING, + /// @description transmits the packet with PacketHandler::txRxPacket(), + /// @description and call PacketHandler::readTxRx to read model_number in the + /// rx buffer. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return COMM_SUCCESS + /// @return when it succeeds to ping Dynamixel and get model_number from it + /// @return or the other communication results which come from + /// PacketHandler::txRxPacket() and PacketHandler::readTxRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int ping(PortHandler *port, uint8_t id, uint16_t *model_number, + uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that pings all + /// connected Dynamixel + /// @param port PortHandler instance + /// @param id_list ID list of Dynamixels which are found by broadcast ping + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + virtual int broadcastPing(PortHandler *port, + std::vector &id_list) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixels run as written in the Dynamixel + /// register + /// @description The function makes an instruction packet with INST_ACTION, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description To use this function, Dynamixel register should be set by + /// PacketHandler::regWriteTxOnly() or PacketHandler::regWriteTxRx() + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int action(PortHandler *port, uint8_t id) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reboot + /// @description The function makes an instruction packet with INST_REBOOT, + /// @description transmits the packet with PacketHandler::txRxPacket(), + /// @description then Dynamixel reboots. + /// @description During reboot, its LED will blink. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + virtual int reboot(PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reset multi-turn revolution information of + /// Dynamixel + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or + /// above), + /// @description Dynamixel X-series (Firmware v42 or above). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int clearMultiTurn(PortHandler *port, uint8_t id, + uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reset as it was produced in the + /// factory + /// @description The function makes an instruction packet with + /// INST_FACTORY_RESET, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description Be careful of the use. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param option Reset option + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int factoryReset(PortHandler *port, uint8_t id, uint8_t option = 0, + uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from + /// PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int readTx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet and reads the data in the + /// packet + /// @description The function receives the packet which might be come by + /// previous INST_READ instruction packet transmission, + /// @description gets the data from the packet. + /// @param port PortHandler instance + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int readRx(PortHandler *port, uint8_t id, uint16_t length, + uint8_t *data, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet, and read + /// data from received packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits and receives the packet with + /// PacketHandler::txRxPacket(), + /// @description gets the data from the packet. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from + /// PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int readTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTx() function for + /// reading 1 byte data + /// @description The function calls PacketHandler::readTx() function for + /// reading 1 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readRx() function and reads + /// 1 byte data on the packet + /// @description The function calls PacketHandler::readRx() function, + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, + uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTxRx() function for + /// reading 1 byte data + /// @description The function calls PacketHandler::readTxRx(), + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint8_t *data, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTx() function for + /// reading 2 byte data + /// @description The function calls PacketHandler::readTx() function for + /// reading 2 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readRx() function and reads + /// 2 byte data on the packet + /// @description The function calls PacketHandler::readRx() function, + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, + uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTxRx() function for + /// reading 2 byte data + /// @description The function calls PacketHandler::readTxRx(), + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t *data, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTx() function for + /// reading 4 byte data + /// @description The function calls PacketHandler::readTx() function for + /// reading 4 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readRx() function and reads + /// 4 byte data on the packet + /// @description The function calls PacketHandler::readRx() function, + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, + uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTxRx() function for + /// reading 4 byte data + /// @description The function calls PacketHandler::readTxRx(), + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint32_t *data, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the + /// data for write + /// @description The function makes an instruction packet with INST_WRITE and + /// the data for write, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the + /// data for write, and receives the packet + /// @description The function makes an instruction packet with INST_WRITE and + /// the data for write, + /// @description transmits and receives the packet with + /// PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int writeTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxOnly() for writing 1 + /// byte data + /// @description The function calls PacketHandler::writeTxOnly() for writing 1 + /// byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint8_t data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxRx() for writing 1 + /// byte data and receives the packet + /// @description The function calls PacketHandler::writeTxRx() for writing 1 + /// byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint8_t data, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxOnly() for writing 2 + /// byte data + /// @description The function calls PacketHandler::writeTxOnly() for writing 2 + /// byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint16_t data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxRx() for writing 2 + /// byte data and receives the packet + /// @description The function calls PacketHandler::writeTxRx() for writing 2 + /// byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t data, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxOnly() for writing 4 + /// byte data + /// @description The function calls PacketHandler::writeTxOnly() for writing 4 + /// byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint32_t data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxRx() for writing 4 + /// byte data and receives the packet + /// @description The function calls PacketHandler::writeTxRx() for writing 4 + /// byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint32_t data, uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with + /// the data for writing on the Dynamixel register + /// @description The function makes an instruction packet with INST_REG_WRITE + /// and the data for writing on the Dynamixel register, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @description The data written in the register will act when INST_ACTION + /// instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with + /// the data for writing on the Dynamixel register, and receives the packet + /// @description The function makes an instruction packet with INST_REG_WRITE + /// and the data for writing on the Dynamixel register, + /// @description transmits and receives the packet with + /// PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @description The data written in the register will act when INST_ACTION + /// instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data, + uint8_t *error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_SYNC_READ instruction packet + /// @description The function makes an instruction packet with INST_SYNC_READ, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Read + /// @param data_length Length of the data for Sync Read + /// @param param Parameter for Sync Read + /// @param param_length Length of the data for Sync Read + /// @return communication results which come from PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int syncReadTx(PortHandler *port, uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length) = 0; + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_SYNC_WRITE instruction packet + /// @description The function makes an instruction packet with + /// INST_SYNC_WRITE, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Write + /// @param data_length Length of the data for Sync Write + /// @param param Parameter for Sync Write + /// @param param_length Length of the data for Sync Write + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int syncWriteTxOnly(PortHandler *port, uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_BULK_READ instruction packet + /// @description The function makes an instruction packet with INST_BULK_READ, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Read + /// @param param_length Length of the data for Bulk Read + /// @return communication results which come from PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int bulkReadTx(PortHandler *port, uint8_t *param, + uint16_t param_length) = 0; + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_BULK_WRITE instruction packet + /// @description The function makes an instruction packet with + /// INST_BULK_WRITE, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Write + /// @param param_length Length of the data for Bulk Write + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int bulkWriteTxOnly(PortHandler *port, uint8_t *param, + uint16_t param_length) = 0; + + virtual int fastSyncReadTx(PortHandler *port, uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length) = 0; + virtual int fastBulkReadTx(PortHandler *port, uint8_t *param, + uint16_t param_length) = 0; +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler.cpp b/src/drivers/dynamixel/port_handler.cpp new file mode 100644 index 0000000..38bcd02 --- /dev/null +++ b/src/drivers/dynamixel/port_handler.cpp @@ -0,0 +1,48 @@ +/******************************************************************************* + * 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: zerom, Ryu Woon Jung (Leon) */ + +#if defined(__linux__) +#include "port_handler.h" +#include "port_handler_linux.h" +#elif defined(__APPLE__) +#include "port_handler.h" +#include "port_handler_mac.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "port_handler.h" +#include "port_handler_windows.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include "../../include/dynamixel_sdk/port_handler.h" +#include "../../include/dynamixel_sdk/port_handler_arduino.h" +#endif + +using namespace dynamixel; + +PortHandler *PortHandler::getPortHandler(const char *port_name) { +#if defined(__linux__) + return (PortHandler *)(new PortHandlerLinux(port_name)); +#elif defined(__APPLE__) + return (PortHandler *)(new PortHandlerMac(port_name)); +#elif defined(_WIN32) || defined(_WIN64) + return (PortHandler *)(new PortHandlerWindows(port_name)); +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) + return (PortHandler *)(new PortHandlerArduino(port_name)); +#endif +} \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler.h b/src/drivers/dynamixel/port_handler.h new file mode 100644 index 0000000..cb617e0 --- /dev/null +++ b/src/drivers/dynamixel/port_handler.h @@ -0,0 +1,190 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for port control +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ + +#if defined(__linux__) +#define WINDECLSPEC +#elif defined(__APPLE__) +#define WINDECLSPEC +#elif defined(_WIN32) || defined(_WIN64) +#ifdef WINDLLEXPORT +#define WINDECLSPEC __declspec(dllexport) +#else +#define WINDECLSPEC __declspec(dllimport) +#endif +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#define WINDECLSPEC +#endif + +#ifdef __GNUC__ +#define DEPRECATED __attribute__((deprecated)) +#elif defined(_MSC_VER) +#define DEPRECATED __declspec(deprecated) +#else +#pragma message("WARNING: You need to implement DEPRECATED for this compiler") +#define DEPRECATED +#endif + +#include + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for port control that inherits PortHandlerLinux, +/// PortHandlerWindows, PortHandlerMac, or PortHandlerArduino +//////////////////////////////////////////////////////////////////////////////// +class WINDECLSPEC PortHandler { +public: + static const int DEFAULT_BAUDRATE_ = 57600; ///< Default Baudrate + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets PortHandler class inheritance + /// @description The function gets class inheritance (PortHandlerLinux / + /// PortHandlerWindows / PortHandlerMac / PortHandlerArduino. + //////////////////////////////////////////////////////////////////////////////// + static PortHandler *getPortHandler(const char *port_name); + + bool is_using_; ///< shows whether the port is in use + + virtual ~PortHandler() {} + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerLinux::setBaudRate() to open + /// the port. + /// @return communication results which come from + /// PortHandlerLinux::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + virtual bool openPort() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + virtual void closePort() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + virtual void clearPort() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + virtual void setPortName(const char *port_name) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port + /// handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + virtual char *getPortName() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + virtual bool setBaudRate(const int baudrate) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port + /// handler + /// @description The function returns current baudrate set into the port + /// handler. + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + virtual int getBaudRate() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from + /// the port buffer + /// @description The function checks how much bytes are able to be read from + /// the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + virtual int getBytesAvailable() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + virtual int readPort(uint8_t *packet, int length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + virtual int writePort(uint8_t *packet, int length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + virtual void setPacketTimeout(uint16_t packet_length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + virtual void setPacketTimeout(double msec) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the + /// time of packet timeout from the time set by + /// PortHandlerLinux::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + virtual bool isPacketTimeout() = 0; +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler_arduino.cpp b/src/drivers/dynamixel/port_handler_arduino.cpp new file mode 100644 index 0000000..a9f1286 --- /dev/null +++ b/src/drivers/dynamixel/port_handler_arduino.cpp @@ -0,0 +1,269 @@ +/******************************************************************************* + * 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) */ + +#if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) + +#include + +#include "../../include/dynamixel_sdk/port_handler_arduino.h" + +#if defined(__OPENCR__) +#define DYNAMIXEL_SERIAL Serial3 +#endif + +#if defined(ARDUINO_OpenRB) +#define DYNAMIXEL_SERIAL Serial1 +#endif + +#define LATENCY_TIMER 4 // msec (USB latency timer) + +using namespace dynamixel; + +PortHandlerArduino::PortHandlerArduino(const char *port_name) + : baudrate_(DEFAULT_BAUDRATE_), packet_start_time_(0.0), + packet_timeout_(0.0), tx_time_per_byte(0.0) { + is_using_ = false; + setPortName(port_name); + +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + pinMode(BDPIN_DXL_PWR_EN, OUTPUT); + + setPowerOff(); +#elif defined(__OPENCM904__) + if (port_name[0] == '1') { + socket_fd_ = 0; + p_dxl_serial = &Serial1; + } else if (port_name[0] == '2') { + socket_fd_ = 1; + p_dxl_serial = &Serial2; + } else if (port_name[0] == '3') { + socket_fd_ = 2; + p_dxl_serial = &Serial3; + } else { + socket_fd_ = 0; + p_dxl_serial = &Serial1; + } + + drv_dxl_begin(socket_fd_); +#endif + + setTxDisable(); +} + +bool PortHandlerArduino::openPort() { +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + pinMode(BDPIN_DXL_PWR_EN, OUTPUT); + + setPowerOn(); + delay(1000); +#endif + + return setBaudRate(baudrate_); +} + +void PortHandlerArduino::closePort() { setPowerOff(); } + +void PortHandlerArduino::clearPort() { + int temp __attribute__((unused)); +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + while (DYNAMIXEL_SERIAL.available()) { + temp = DYNAMIXEL_SERIAL.read(); + } +#elif defined(__OPENCM904__) + while (p_dxl_serial->available()) { + temp = p_dxl_serial->read(); + } +#endif +} + +void PortHandlerArduino::setPortName(const char *port_name) { + strcpy(port_name_, port_name); +} + +char *PortHandlerArduino::getPortName() { return port_name_; } + +bool PortHandlerArduino::setBaudRate(const int baudrate) { + baudrate_ = checkBaudrateAvailable(baudrate); + + if (baudrate_ == -1) + return false; + + setupPort(baudrate_); + + return true; +} + +int PortHandlerArduino::getBaudRate() { return baudrate_; } + +int PortHandlerArduino::getBytesAvailable() { + int bytes_available; + +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + bytes_available = DYNAMIXEL_SERIAL.available(); +#elif defined(__OPENCM904__) + bytes_available = p_dxl_serial->available(); +#endif + + return bytes_available; +} + +int PortHandlerArduino::readPort(uint8_t *packet, int length) { + int rx_length; + +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + rx_length = DYNAMIXEL_SERIAL.available(); +#elif defined(__OPENCM904__) + rx_length = p_dxl_serial->available(); +#endif + + if (rx_length > length) + rx_length = length; + + for (int i = 0; i < rx_length; i++) { +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + packet[i] = DYNAMIXEL_SERIAL.read(); +#elif defined(__OPENCM904__) + packet[i] = p_dxl_serial->read(); +#endif + } + + return rx_length; +} + +int PortHandlerArduino::writePort(uint8_t *packet, int length) { + int length_written; + + setTxEnable(); + +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + length_written = DYNAMIXEL_SERIAL.write(packet, length); +#elif defined(__OPENCM904__) + length_written = p_dxl_serial->write(packet, length); +#endif + + setTxDisable(); + + return length_written; +} + +void PortHandlerArduino::setPacketTimeout(uint16_t packet_length) { + packet_start_time_ = getCurrentTime(); + packet_timeout_ = + (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; +} + +void PortHandlerArduino::setPacketTimeout(double msec) { + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; +} + +bool PortHandlerArduino::isPacketTimeout() { + if (getTimeSinceStart() > packet_timeout_) { + packet_timeout_ = 0; + return true; + } + + return false; +} + +double PortHandlerArduino::getCurrentTime() { return (double)millis(); } + +double PortHandlerArduino::getTimeSinceStart() { + double elapsed_time; + + elapsed_time = getCurrentTime() - packet_start_time_; + if (elapsed_time < 0.0) + packet_start_time_ = getCurrentTime(); + + return elapsed_time; +} + +bool PortHandlerArduino::setupPort(int baudrate) { +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + DYNAMIXEL_SERIAL.begin(baudrate); +#elif defined(__OPENCM904__) + p_dxl_serial->setDxlMode(true); + p_dxl_serial->begin(baudrate); +#endif + + delay(100); + + tx_time_per_byte = (1000.0 / (double)baudrate) * 10.0; + return true; +} + +int PortHandlerArduino::checkBaudrateAvailable(int baudrate) { + switch (baudrate) { + case 9600: + return 9600; + case 57600: + return 57600; + case 115200: + return 115200; + case 1000000: + return 1000000; + case 2000000: + return 2000000; + case 3000000: + return 3000000; + case 4000000: + return 4000000; + case 4500000: + return 4500000; + default: + return -1; + } +} + +void PortHandlerArduino::setPowerOn() { +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + digitalWrite(BDPIN_DXL_PWR_EN, HIGH); +#endif +} + +void PortHandlerArduino::setPowerOff() { +#if defined(__OPENCR__) || defined(ARDUINO_OpenRB) + digitalWrite(BDPIN_DXL_PWR_EN, LOW); +#endif +} + +void PortHandlerArduino::setTxEnable() { +#if defined(__OPENCR__) + drv_dxl_tx_enable(TRUE); +#elif defined(__OPENCM904__) + drv_dxl_tx_enable(socket_fd_, TRUE); +#endif +} + +void PortHandlerArduino::setTxDisable() { +#if defined(__OPENCR__) +#ifdef SERIAL_WRITES_NON_BLOCKING + DYNAMIXEL_SERIAL.flush(); // make sure it completes before we disable... +#endif + drv_dxl_tx_enable(FALSE); + +#elif defined(__OPENCM904__) +#ifdef SERIAL_WRITES_NON_BLOCKING + p_dxl_serial->flush(); +#endif + drv_dxl_tx_enable(socket_fd_, FALSE); +#endif +} + +#endif \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler_arduino.h b/src/drivers/dynamixel/port_handler_arduino.h new file mode 100644 index 0000000..bcb0858 --- /dev/null +++ b/src/drivers/dynamixel/port_handler_arduino.h @@ -0,0 +1,197 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for port control in Arduino +/// @author Cho (Hancheol Cho), Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ARDUINO_PORTHANDLERARDUINO_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ARDUINO_PORTHANDLERARDUINO_H_ + +#if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include +#endif + +#include "port_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for control port in Arduino +//////////////////////////////////////////////////////////////////////////////// +class PortHandlerArduino : public PortHandler { +private: + int socket_fd_; + int baudrate_; + char port_name_[100]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte; + +#if defined(__OPENCM904__) + UARTClass *p_dxl_serial; +#endif + + bool setupPort(const int cflag_baud); + + double getCurrentTime(); + double getTimeSinceStart(); + + int checkBaudrateAvailable(int baudrate); + + void setPowerOn(); + void setPowerOff(); + void setTxEnable(); + void setTxDisable(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that initializes instance of PortHandler and gets + /// port_name + /// @description The function initializes instance of PortHandler and gets + /// port_name. + //////////////////////////////////////////////////////////////////////////////// + PortHandlerArduino(const char *port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function calls PortHandlerArduino::closePort() to close + /// the port. + //////////////////////////////////////////////////////////////////////////////// + virtual ~PortHandlerArduino() { closePort(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerArduino::setBaudRate() to open + /// the port. + /// @return communication results which come from + /// PortHandlerArduino::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + bool openPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + void closePort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + void clearPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + void setPortName(const char *port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port + /// handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + char *getPortName(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool setBaudRate(const int baudrate); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port + /// handler + /// @description The function returns current baudrate set into the port + /// handler. + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + int getBaudRate(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from + /// the port buffer + /// @description The function checks how much bytes are able to be read from + /// the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + int getBytesAvailable(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + int readPort(uint8_t *packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + int writePort(uint8_t *packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(uint16_t packet_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(double msec); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the + /// time of packet timeout from the time set by + /// PortHandlerArduino::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + bool isPacketTimeout(); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ARDUINO_PORTHANDLERARDUINO_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler_linux.cpp b/src/drivers/dynamixel/port_handler_linux.cpp new file mode 100644 index 0000000..8330afe --- /dev/null +++ b/src/drivers/dynamixel/port_handler_linux.cpp @@ -0,0 +1,288 @@ +/******************************************************************************* + * 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: zerom, Ryu Woon Jung (Leon) */ + +#if defined(__linux__) + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "port_handler_linux.h" + +#define LATENCY_TIMER \ + 16 // msec (USB latency timer) + // You should adjust the latency timer value. From the version + // Ubuntu 16.04.2, the default latency timer of the usb serial is '16 + // msec'. When you are going to use sync / bulk read, the latency timer + // should be loosen. the lower latency timer value, the faster + // communication speed. + +// Note: +// You can check its value by: +// $ cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +// +// If you think that the communication is too slow, type following after +// plugging the usb in to change the latency timer +// +// Method 1. Type following (you should do this everytime when the usb once was +// plugged out or the connection was dropped) $ echo 1 | sudo tee +// /sys/bus/usb-serial/devices/ttyUSB0/latency_timer $ cat +// /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +// +// Method 2. If you want to set it as be done automatically, and don't want to +// do above everytime, make rules file in /etc/udev/rules.d/. For example, $ +// echo ACTION==\"add\", SUBSYSTEM==\"usb-serial\", DRIVER==\"ftdi_sio\", +// ATTR{latency_timer}=\"1\" > 99-dynamixelsdk-usb.rules $ sudo cp +// ./99-dynamixelsdk-usb.rules /etc/udev/rules.d/ $ sudo udevadm control +// --reload-rules $ sudo udevadm trigger --action=add $ cat +// /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +// +// or if you have another good idea that can be an alternatives, +// please give us advice via github issue +// https://github.com/ROBOTIS-GIT/DynamixelSDK/issues + +struct termios2 { + tcflag_t c_iflag; /* input mode flags */ + tcflag_t c_oflag; /* output mode flags */ + tcflag_t c_cflag; /* control mode flags */ + tcflag_t c_lflag; /* local mode flags */ + cc_t c_line; /* line discipline */ + cc_t c_cc[19]; /* control characters */ + speed_t c_ispeed; /* input speed */ + speed_t c_ospeed; /* output speed */ +}; + +#ifndef TCGETS2 +#define TCGETS2 _IOR('T', 0x2A, struct termios2) +#endif +#ifndef TCSETS2 +#define TCSETS2 _IOW('T', 0x2B, struct termios2) +#endif +#ifndef BOTHER +#define BOTHER 0010000 +#endif + +using namespace dynamixel; + +PortHandlerLinux::PortHandlerLinux(const char *port_name) + : socket_fd_(-1), baudrate_(DEFAULT_BAUDRATE_), packet_start_time_(0.0), + packet_timeout_(0.0), tx_time_per_byte(0.0) { + is_using_ = false; + setPortName(port_name); +} + +bool PortHandlerLinux::openPort() { return setBaudRate(baudrate_); } + +void PortHandlerLinux::closePort() { + if (socket_fd_ != -1) + close(socket_fd_); + socket_fd_ = -1; +} + +void PortHandlerLinux::clearPort() { tcflush(socket_fd_, TCIFLUSH); } + +void PortHandlerLinux::setPortName(const char *port_name) { + strcpy(port_name_, port_name); +} + +char *PortHandlerLinux::getPortName() { return port_name_; } + +// TODO: baud number ?? +bool PortHandlerLinux::setBaudRate(const int baudrate) { + int baud = getCFlagBaud(baudrate); + + closePort(); + + if (baud <= 0) // custom baudrate + { + setupPort(B38400); + baudrate_ = baudrate; + return setCustomBaudrate(baudrate); + } else { + baudrate_ = baudrate; + return setupPort(baud); + } +} + +int PortHandlerLinux::getBaudRate() { return baudrate_; } + +int PortHandlerLinux::getBytesAvailable() { + int bytes_available; + ioctl(socket_fd_, FIONREAD, &bytes_available); + return bytes_available; +} + +int PortHandlerLinux::readPort(uint8_t *packet, int length) { + return read(socket_fd_, packet, length); +} + +int PortHandlerLinux::writePort(uint8_t *packet, int length) { + return write(socket_fd_, packet, length); +} + +void PortHandlerLinux::setPacketTimeout(uint16_t packet_length) { + packet_start_time_ = getCurrentTime(); + packet_timeout_ = + (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; +} + +void PortHandlerLinux::setPacketTimeout(double msec) { + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; +} + +bool PortHandlerLinux::isPacketTimeout() { + if (getTimeSinceStart() > packet_timeout_) { + packet_timeout_ = 0; + return true; + } + return false; +} + +double PortHandlerLinux::getCurrentTime() { + struct timespec tv; + clock_gettime(CLOCK_REALTIME, &tv); + return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); +} + +double PortHandlerLinux::getTimeSinceStart() { + double time; + + time = getCurrentTime() - packet_start_time_; + if (time < 0.0) + packet_start_time_ = getCurrentTime(); + + return time; +} + +bool PortHandlerLinux::setupPort(int cflag_baud) { + struct termios newtio; + + socket_fd_ = open(port_name_, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (socket_fd_ < 0) { + printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); + return false; + } + + bzero(&newtio, sizeof(newtio)); // clear struct for new port settings + + newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; + + // clean the buffer and activate the settings for the port + tcflush(socket_fd_, TCIFLUSH); + tcsetattr(socket_fd_, TCSANOW, &newtio); + + tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + return true; +} + +bool PortHandlerLinux::setCustomBaudrate(int speed) { + struct termios2 options; + + if (ioctl(socket_fd_, TCGETS2, &options) != 01) { + options.c_cflag &= ~CBAUD; + options.c_cflag |= BOTHER; + options.c_ispeed = speed; + options.c_ospeed = speed; + + if (ioctl(socket_fd_, TCSETS2, &options) != -1) + return true; + } + + // try to set a custom divisor + struct serial_struct ss; + if (ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); + return false; + } + + ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; + ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; + int closest_speed = ss.baud_base / ss.custom_divisor; + + if (closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) { + printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, " + "closest is %d \n", + speed, closest_speed); + return false; + } + + if (ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); + return false; + } + + tx_time_per_byte = (1000.0 / (double)speed) * 10.0; + return true; +} + +int PortHandlerLinux::getCFlagBaud(int baudrate) { + switch (baudrate) { + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + case 460800: + return B460800; + case 500000: + return B500000; + case 576000: + return B576000; + case 921600: + return B921600; + case 1000000: + return B1000000; + case 1152000: + return B1152000; + case 1500000: + return B1500000; + case 2000000: + return B2000000; + case 2500000: + return B2500000; + case 3000000: + return B3000000; + case 3500000: + return B3500000; + case 4000000: + return B4000000; + default: + return -1; + } +} + +#endif \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler_linux.h b/src/drivers/dynamixel/port_handler_linux.h new file mode 100644 index 0000000..4229033 --- /dev/null +++ b/src/drivers/dynamixel/port_handler_linux.h @@ -0,0 +1,183 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for port control in Linux +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ + +#include "port_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for control port in Linux +//////////////////////////////////////////////////////////////////////////////// +class PortHandlerLinux : public PortHandler { +private: + int socket_fd_; + int baudrate_; + char port_name_[100]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte; + + bool setupPort(const int cflag_baud); + bool setCustomBaudrate(int speed); + int getCFlagBaud(const int baudrate); + + double getCurrentTime(); + double getTimeSinceStart(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that initializes instance of PortHandler and gets + /// port_name + /// @description The function initializes instance of PortHandler and gets + /// port_name. + //////////////////////////////////////////////////////////////////////////////// + PortHandlerLinux(const char *port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function calls PortHandlerLinux::closePort() to close the + /// port. + //////////////////////////////////////////////////////////////////////////////// + virtual ~PortHandlerLinux() { closePort(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerLinux::setBaudRate() to open + /// the port. + /// @return communication results which come from + /// PortHandlerLinux::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + bool openPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + void closePort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + void clearPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + void setPortName(const char *port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port + /// handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + char *getPortName(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool setBaudRate(const int baudrate); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port + /// handler + /// @description The function returns current baudrate set into the port + /// handler. + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + int getBaudRate(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from + /// the port buffer + /// @description The function checks how much bytes are able to be read from + /// the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + int getBytesAvailable(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + int readPort(uint8_t *packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + int writePort(uint8_t *packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(uint16_t packet_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(double msec); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the + /// time of packet timeout from the time set by + /// PortHandlerLinux::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + bool isPacketTimeout(); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler_mac.cpp b/src/drivers/dynamixel/port_handler_mac.cpp new file mode 100644 index 0000000..0aa1228 --- /dev/null +++ b/src/drivers/dynamixel/port_handler_mac.cpp @@ -0,0 +1,238 @@ +/******************************************************************************* + * 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) */ + +#if defined(__APPLE__) + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef __MACH__ +#include +#include +#endif + +#include "port_handler_mac.h" + +#define LATENCY_TIMER \ + 16 // msec (USB latency timer) + // You should adjust the latency timer value. + // When you are going to use sync / bulk read, the latency timer should be + // loosen. the lower latency timer value, the faster communication speed. + +// Note: +// You can either change its value by following: +// http://www.ftdichip.com/Support/Documents/TechnicalNotes/TN_105%20Adding%20Support%20for%20New%20FTDI%20Devices%20to%20Mac%20Driver.pdf + +using namespace dynamixel; + +PortHandlerMac::PortHandlerMac(const char *port_name) + : socket_fd_(-1), baudrate_(DEFAULT_BAUDRATE_), packet_start_time_(0.0), + packet_timeout_(0.0), tx_time_per_byte(0.0) { + is_using_ = false; + setPortName(port_name); +} + +bool PortHandlerMac::openPort() { return setBaudRate(baudrate_); } + +void PortHandlerMac::closePort() { + if (socket_fd_ != -1) + close(socket_fd_); + socket_fd_ = -1; +} + +void PortHandlerMac::clearPort() { tcflush(socket_fd_, TCIFLUSH); } + +void PortHandlerMac::setPortName(const char *port_name) { + strcpy(port_name_, port_name); +} + +char *PortHandlerMac::getPortName() { return port_name_; } + +// TODO: baud number ?? +bool PortHandlerMac::setBaudRate(const int baudrate) { + int baud = getCFlagBaud(baudrate); + + closePort(); + + if (baud <= 0) // custom baudrate + { + setupPort(B38400); + baudrate_ = baudrate; + return setCustomBaudrate(baudrate); + } else { + baudrate_ = baudrate; + return setupPort(baud); + } +} + +int PortHandlerMac::getBaudRate() { return baudrate_; } + +int PortHandlerMac::getBytesAvailable() { + int bytes_available; + ioctl(socket_fd_, FIONREAD, &bytes_available); + return bytes_available; +} + +int PortHandlerMac::readPort(uint8_t *packet, int length) { + return read(socket_fd_, packet, length); +} + +int PortHandlerMac::writePort(uint8_t *packet, int length) { + int result = write(socket_fd_, packet, length); + + // Wait Tx Done + int unsent = 1; + while (unsent > 0) { + ioctl(socket_fd_, TIOCOUTQ, &unsent); + } + unsigned int tx_time = ((float)(length * 10) / baudrate_) * 1000000000; + struct timespec delay = {0, tx_time}; + nanosleep(&delay, NULL); + + return result; +} + +void PortHandlerMac::setPacketTimeout(uint16_t packet_length) { + packet_start_time_ = getCurrentTime(); + packet_timeout_ = + (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; +} + +void PortHandlerMac::setPacketTimeout(double msec) { + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; +} + +bool PortHandlerMac::isPacketTimeout() { + if (getTimeSinceStart() > packet_timeout_) { + packet_timeout_ = 0; + return true; + } + return false; +} + +double PortHandlerMac::getCurrentTime() { + struct timespec tv; +#ifdef __MACH__ // OS X does not have clock_gettime, so here uses clock_get_time + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + tv.tv_sec = mts.tv_sec; + tv.tv_nsec = mts.tv_nsec; +#else + clock_gettime(CLOCK_REALTIME, &tv); +#endif + return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); +} + +double PortHandlerMac::getTimeSinceStart() { + double time; + + time = getCurrentTime() - packet_start_time_; + if (time < 0.0) + packet_start_time_ = getCurrentTime(); + + return time; +} + +bool PortHandlerMac::setupPort(int cflag_baud) { + struct termios newtio; + + socket_fd_ = open(port_name_, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (socket_fd_ < 0) { + printf("[PortHandlerMac::SetupPort] Error opening serial port!\n"); + return false; + } + + bzero(&newtio, sizeof(newtio)); // clear struct for new port settings + + newtio.c_cflag = CS8 | CLOCAL | CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; + cfsetispeed(&newtio, cflag_baud); + cfsetospeed(&newtio, cflag_baud); + + // clean the buffer and activate the settings for the port + tcflush(socket_fd_, TCIFLUSH); + tcsetattr(socket_fd_, TCSANOW, &newtio); + + tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + return true; +} + +bool PortHandlerMac::setCustomBaudrate(int speed) { + printf("[PortHandlerMac::SetCustomBaudrate] Not supported on Mac!\n"); + return false; +} + +int PortHandlerMac::getCFlagBaud(int baudrate) { + switch (baudrate) { + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + // Mac OS doesn't support over B230400 + // case 460800: + // return B460800; + // case 500000: + // return B500000; + // case 576000: + // return B576000; + // case 921600: + // return B921600; + // case 1000000: + // return B1000000; + // case 1152000: + // return B1152000; + // case 1500000: + // return B1500000; + // case 2000000: + // return B2000000; + // case 2500000: + // return B2500000; + // case 3000000: + // return B3000000; + // case 3500000: + // return B3500000; + // case 4000000: + // return B4000000; + default: + return -1; + } +} + +#endif \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler_mac.h b/src/drivers/dynamixel/port_handler_mac.h new file mode 100644 index 0000000..74e5969 --- /dev/null +++ b/src/drivers/dynamixel/port_handler_mac.h @@ -0,0 +1,184 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for port control in Mac OS +/// @author Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_MAC_PORTHANDLERMAC_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_MAC_PORTHANDLERMAC_H_ + +#include "port_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for control port in Mac OS +//////////////////////////////////////////////////////////////////////////////// +class PortHandlerMac : public PortHandler { +private: + int socket_fd_; + int baudrate_; + char port_name_[100]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte; + + bool setupPort(const int cflag_baud); + bool setCustomBaudrate(int speed); + int getCFlagBaud(const int baudrate); + + double getCurrentTime(); + double getTimeSinceStart(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that initializes instance of PortHandler and gets + /// port_name + /// @description The function initializes instance of PortHandler and gets + /// port_name. + //////////////////////////////////////////////////////////////////////////////// + PortHandlerMac(const char *port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function calls PortHandlerMac::closePort() to close the + /// port. + //////////////////////////////////////////////////////////////////////////////// + virtual ~PortHandlerMac() { closePort(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerMac::setBaudRate() to open the + /// port. + /// @return communication results which come from + /// PortHandlerMac::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + bool openPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + void closePort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + void clearPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + void setPortName(const char *port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port + /// handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + char *getPortName(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool setBaudRate(const int baudrate); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port + /// handler + /// @description The function returns current baudrate set into the port + /// handler. + /// @warning Mac OS doesn't support over 230400 bps + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + int getBaudRate(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from + /// the port buffer + /// @description The function checks how much bytes are able to be read from + /// the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + int getBytesAvailable(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + int readPort(uint8_t *packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + int writePort(uint8_t *packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(uint16_t packet_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(double msec); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the + /// time of packet timeout from the time set by + /// PortHandlerMac::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + bool isPacketTimeout(); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_MAC_PORTHANDLERMAC_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler_windows.cpp b/src/drivers/dynamixel/port_handler_windows.cpp new file mode 100644 index 0000000..eaa8bf5 --- /dev/null +++ b/src/drivers/dynamixel/port_handler_windows.cpp @@ -0,0 +1,216 @@ +/******************************************************************************* + * 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) */ + +#if defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT + +#include "port_handler_windows.h" + +#include +#include +#include + +#define LATENCY_TIMER \ + 16 // msec (USB latency timer) + // You should adjust the latency timer value. In Windows, the default + // latency timer of the usb serial is '16 msec'. When you are going to use + // sync / bulk read, the latency timer should be loosen. the lower latency + // timer value, the faster communication speed. + +// Note: +// You can either checking or changing its value by: +// [Device Manager] -> [Port (COM & LPT)] -> the port you use but starts with +// COMx-> mouse right click -> properties +// -> [port settings] -> [details] -> change response time from 16 to the value +// you need + +using namespace dynamixel; + +PortHandlerWindows::PortHandlerWindows(const char *port_name) + : serial_handle_(INVALID_HANDLE_VALUE), baudrate_(DEFAULT_BAUDRATE_), + packet_start_time_(0.0), packet_timeout_(0.0), tx_time_per_byte_(0.0) { + is_using_ = false; + + char buffer[15]; + sprintf_s(buffer, sizeof(buffer), "\\\\.\\%s", port_name); + setPortName(buffer); +} + +bool PortHandlerWindows::openPort() { return setBaudRate(baudrate_); } + +void PortHandlerWindows::closePort() { + if (serial_handle_ != INVALID_HANDLE_VALUE) { + CloseHandle(serial_handle_); + serial_handle_ = INVALID_HANDLE_VALUE; + } +} + +void PortHandlerWindows::clearPort() { + PurgeComm(serial_handle_, PURGE_RXABORT | PURGE_RXCLEAR); +} + +void PortHandlerWindows::setPortName(const char *port_name) { + strcpy_s(port_name_, sizeof(port_name_), port_name); +} + +char *PortHandlerWindows::getPortName() { return port_name_; } + +bool PortHandlerWindows::setBaudRate(const int baudrate) { + closePort(); + + baudrate_ = baudrate; + return setupPort(baudrate); +} + +int PortHandlerWindows::getBaudRate() { return baudrate_; } + +int PortHandlerWindows::getBytesAvailable() { + DWORD retbyte = 2; + BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, + 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL); + + printf("%d", (int)res); + return (int)retbyte; +} + +int PortHandlerWindows::readPort(uint8_t *packet, int length) { + DWORD dwRead = 0; + + if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE) + return -1; + + return (int)dwRead; +} + +int PortHandlerWindows::writePort(uint8_t *packet, int length) { + DWORD dwWrite = 0; + + if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE) + return -1; + + return (int)dwWrite; +} + +void PortHandlerWindows::setPacketTimeout(uint16_t packet_length) { + packet_start_time_ = getCurrentTime(); + packet_timeout_ = + (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; +} + +void PortHandlerWindows::setPacketTimeout(double msec) { + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; +} + +bool PortHandlerWindows::isPacketTimeout() { + if (getTimeSinceStart() > packet_timeout_) { + packet_timeout_ = 0; + return true; + } + return false; +} + +double PortHandlerWindows::getCurrentTime() { + QueryPerformanceCounter(&counter_); + QueryPerformanceFrequency(&freq_); + return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0; +} + +double PortHandlerWindows::getTimeSinceStart() { + double time; + + time = getCurrentTime() - packet_start_time_; + if (time < 0.0) + packet_start_time_ = getCurrentTime(); + + return time; +} + +bool PortHandlerWindows::setupPort(int baudrate) { + DCB dcb; + COMMTIMEOUTS timeouts; + DWORD dwError; + + closePort(); + + serial_handle_ = + CreateFileA(port_name_, GENERIC_READ | GENERIC_WRITE, 0, NULL, + OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); + if (serial_handle_ == INVALID_HANDLE_VALUE) { + printf("[PortHandlerWindows::SetupPort] Error opening serial port!\n"); + return false; + } + + dcb.DCBlength = sizeof(DCB); + if (GetCommState(serial_handle_, &dcb) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + // Set baudrate + dcb.BaudRate = (DWORD)baudrate; + dcb.ByteSize = 8; // Data bit = 8bit + dcb.Parity = NOPARITY; // No parity + dcb.StopBits = ONESTOPBIT; // Stop bit = 1 + dcb.fParity = NOPARITY; // No Parity check + dcb.fBinary = 1; // Binary mode + dcb.fNull = 0; // Get Null byte + dcb.fAbortOnError = 0; + dcb.fErrorChar = 0; + // Not using XOn/XOff + dcb.fOutX = 0; + dcb.fInX = 0; + // Not using H/W flow control + dcb.fDtrControl = DTR_CONTROL_DISABLE; + dcb.fRtsControl = RTS_CONTROL_DISABLE; + dcb.fDsrSensitivity = 0; + dcb.fOutxDsrFlow = 0; + dcb.fOutxCtsFlow = 0; + + if (SetCommState(serial_handle_, &dcb) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + if (SetCommMask(serial_handle_, 0) == FALSE) // Not using Comm event + goto DXL_HAL_OPEN_ERROR; + if (SetupComm(serial_handle_, 4096, 4096) == FALSE) // Buffer size (Rx,Tx) + goto DXL_HAL_OPEN_ERROR; + if (PurgeComm(serial_handle_, PURGE_TXABORT | PURGE_TXCLEAR | PURGE_RXABORT | + PURGE_RXCLEAR) == FALSE) // Clear buffer + goto DXL_HAL_OPEN_ERROR; + if (ClearCommError(serial_handle_, &dwError, NULL) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + if (GetCommTimeouts(serial_handle_, &timeouts) == FALSE) + goto DXL_HAL_OPEN_ERROR; + // Timeout (Not using timeout) + // Immediatly return + timeouts.ReadIntervalTimeout = 0; + timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.ReadTotalTimeoutConstant = 1; // must not be zero. + timeouts.WriteTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 0; + if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0; + return true; + +DXL_HAL_OPEN_ERROR: + closePort(); + return false; +} + +#endif \ No newline at end of file diff --git a/src/drivers/dynamixel/port_handler_windows.h b/src/drivers/dynamixel/port_handler_windows.h new file mode 100644 index 0000000..cdfa78c --- /dev/null +++ b/src/drivers/dynamixel/port_handler_windows.h @@ -0,0 +1,185 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for port control in Windows +/// @author Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ + +#include + +#include "port_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for control port in Windows +//////////////////////////////////////////////////////////////////////////////// +class WINDECLSPEC PortHandlerWindows : public PortHandler { +private: + HANDLE serial_handle_; + LARGE_INTEGER freq_, counter_; + + int baudrate_; + char port_name_[100]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte_; + + bool setupPort(const int baudrate); + + double getCurrentTime(); + double getTimeSinceStart(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that initializes instance of PortHandler and gets + /// port_name + /// @description The function initializes instance of PortHandler and gets + /// port_name. + //////////////////////////////////////////////////////////////////////////////// + PortHandlerWindows(const char *port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function calls PortHandlerWindows::closePort() to close + /// the port. + //////////////////////////////////////////////////////////////////////////////// + virtual ~PortHandlerWindows() { closePort(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerWindows::setBaudRate() to open + /// the port. + /// @return communication results which come from + /// PortHandlerWindows::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + bool openPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + void closePort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + void clearPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + void setPortName(const char *port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port + /// handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + char *getPortName(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool setBaudRate(const int baudrate); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port + /// handler + /// @description The function returns current baudrate set into the port + /// handler. + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + int getBaudRate(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from + /// the port buffer + /// @description The function checks how much bytes are able to be read from + /// the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + int getBytesAvailable(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + int readPort(uint8_t *packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + int writePort(uint8_t *packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(uint16_t packet_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet + /// timeout + /// @description The function sets the stopwatch by getting current time and + /// the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(double msec); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the + /// time of packet timeout from the time set by + /// PortHandlerWindows::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + bool isPacketTimeout(); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/protocol1_packet_handler.cpp b/src/drivers/dynamixel/protocol1_packet_handler.cpp new file mode 100644 index 0000000..331c135 --- /dev/null +++ b/src/drivers/dynamixel/protocol1_packet_handler.cpp @@ -0,0 +1,766 @@ +/******************************************************************************* + * 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: zerom, Ryu Woon Jung (Leon) */ + +#if defined(__linux__) +#include "protocol1_packet_handler.h" +#elif defined(__APPLE__) +#include "protocol1_packet_handler.h" +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "protocol1_packet_handler.h" +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include "../../include/dynamixel_sdk/protocol1_packet_handler.h" +#endif + +#include +#include + +#define TXPACKET_MAX_LEN (250) +#define RXPACKET_MAX_LEN (250) + +///////////////// for Protocol 1.0 Packet ///////////////// +#define PKT_HEADER0 0 +#define PKT_HEADER1 1 +#define PKT_ID 2 +#define PKT_LENGTH 3 +#define PKT_INSTRUCTION 4 +#define PKT_ERROR 4 +#define PKT_PARAMETER0 5 + +///////////////// Protocol 1.0 Error bit ///////////////// +#define ERRBIT_VOLTAGE \ + 1 // Supplied voltage is out of the range (operating volatage set in the + // control table) +#define ERRBIT_ANGLE \ + 2 // Goal position is written out of the range (from CW angle limit to CCW + // angle limit) +#define ERRBIT_OVERHEAT \ + 4 // Temperature is out of the range (operating temperature set in the control + // table) +#define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use. +#define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect. +#define ERRBIT_OVERLOAD \ + 32 // The current load cannot be controlled by the set torque. +#define ERRBIT_INSTRUCTION \ + 64 // Undefined instruction or delivering the action command without the + // reg_write command. + +using namespace dynamixel; + +Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = + new Protocol1PacketHandler(); + +Protocol1PacketHandler::Protocol1PacketHandler() {} + +const char *Protocol1PacketHandler::getTxRxResult(int result) { + switch (result) { + case COMM_SUCCESS: + return "[TxRxResult] Communication success."; + + case COMM_PORT_BUSY: + return "[TxRxResult] Port is in use!"; + + case COMM_TX_FAIL: + return "[TxRxResult] Failed transmit instruction packet!"; + + case COMM_RX_FAIL: + return "[TxRxResult] Failed get status packet from device!"; + + case COMM_TX_ERROR: + return "[TxRxResult] Incorrect instruction packet!"; + + case COMM_RX_WAITING: + return "[TxRxResult] Now recieving status packet!"; + + case COMM_RX_TIMEOUT: + return "[TxRxResult] There is no status packet!"; + + case COMM_RX_CORRUPT: + return "[TxRxResult] Incorrect status packet!"; + + case COMM_NOT_AVAILABLE: + return "[TxRxResult] Protocol does not support This function!"; + + default: + return ""; + } +} + +const char *Protocol1PacketHandler::getRxPacketError(uint8_t 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 ""; +} + +int Protocol1PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) { + uint8_t checksum = 0; + uint8_t total_packet_length = + txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH + uint8_t written_packet_length = 0; + + 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 (uint16_t idx = 2; idx < total_packet_length - 1; + idx++) // except header, checksum + checksum += txpacket[idx]; + txpacket[total_packet_length - 1] = ~checksum; + + // tx packet + port->clearPort(); + written_packet_length = port->writePort(txpacket, total_packet_length); + if (total_packet_length != written_packet_length) { + port->is_using_ = false; + return COMM_TX_FAIL; + } + + return COMM_SUCCESS; +} + +int Protocol1PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket, + bool skip_stuffing) { + int result = COMM_TX_FAIL; + + uint8_t checksum = 0; + uint8_t rx_length = 0; + uint8_t wait_length = + 6; // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) + + while (true) { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (rx_length >= wait_length) { + uint8_t idx = 0; + + // find packet header + for (idx = 0; idx < (rx_length - 1); idx++) { + if (rxpacket[idx] == 0xFF && rxpacket[idx + 1] == 0xFF) + break; + } + + if (idx == 0) // found at the beginning of the packet + { + if (rxpacket[PKT_ID] > 0xFD || // unavailable ID + rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length + rxpacket[PKT_ERROR] > 0x7F) // unavailable Error + { + // remove the first byte in the packet + for (uint16_t s = 0; s < rx_length - 1; s++) + rxpacket[s] = rxpacket[1 + s]; + // memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + 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; + } else { + result = COMM_RX_CORRUPT; + } + break; + } else { + continue; + } + } + + // calculate checksum + for (uint16_t i = 2; i < wait_length - 1; + i++) // except header, checksum + checksum += rxpacket[i]; + checksum = ~checksum; + + // verify checksum + if (rxpacket[wait_length - 1] == checksum) { + result = COMM_SUCCESS; + } else { + result = COMM_RX_CORRUPT; + } + break; + } else { + // remove unnecessary packets + for (uint16_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + // memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= idx; + } + } else { + // check timeout + if (port->isPacketTimeout() == true) { + if (rx_length == 0) { + result = COMM_RX_TIMEOUT; + } else { + result = COMM_RX_CORRUPT; + } + break; + } + } + } + port->is_using_ = false; + + return result; +} + +// NOT for BulkRead instruction +int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, + uint8_t *rxpacket, uint8_t *error) { + int result = COMM_TX_FAIL; + + // tx packet + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + return result; + + // (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 || + txpacket[PKT_INSTRUCTION] == INST_ACTION) { + port->is_using_ = false; + return result; + } + + // set packet timeout + if (txpacket[PKT_INSTRUCTION] == INST_READ) { + port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0 + 1] + 6)); + } else { + port->setPacketTimeout( + (uint16_t)6); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + } + + // rx packet + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]); + + if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + + return result; +} + +int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, + uint8_t *error) { + return ping(port, id, 0, error); +} + +int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, + uint16_t *model_number, uint8_t *error) { + int result = COMM_TX_FAIL; + + uint8_t txpacket[6] = {0}; + uint8_t rxpacket[6] = {0}; + + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_PING; + + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS && model_number != 0) { + uint8_t data_read[2] = {0}; + result = readTxRx(port, id, 0, 2, data_read); // Address 0 : Model Number + if (result == COMM_SUCCESS) + *model_number = DXL_MAKEWORD(data_read[0], data_read[1]); + } + + return result; +} + +int Protocol1PacketHandler::broadcastPing(PortHandler *port, + std::vector &id_list) { + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::action(PortHandler *port, uint8_t id) { + uint8_t txpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_ACTION; + + return txRxPacket(port, txpacket, 0); +} + +int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, + uint8_t *error) { + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, + uint8_t *error) { + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, + uint8_t option, uint8_t *error) { + uint8_t txpacket[6] = {0}; + uint8_t rxpacket[6] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; + + return txRxPacket(port, txpacket, rxpacket, error); +} + +int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length) { + int result = COMM_TX_FAIL; + + uint8_t txpacket[8] = {0}; + + 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] = (uint8_t)address; + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)length; + + result = txPacket(port, txpacket); + + // set packet timeout + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)(length + 6)); + + return result; +} + +int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, + uint16_t length, uint8_t *data, + uint8_t *error) { + int result = COMM_TX_FAIL; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length+6); + // uint8_t *rxpacket = new uint8_t[length+6]; + + if (rxpacket == NULL) + return result; + + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); + + if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { + if (error != 0) { + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + for (uint16_t s = 0; s < length; s++) { + data[s] = rxpacket[PKT_PARAMETER0 + s]; + } + // memcpy(data, &rxpacket[PKT_PARAMETER0], length); + } + + free(rxpacket); + // delete[] rxpacket; + return result; +} + +int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error) { + int result = COMM_TX_FAIL; + + uint8_t txpacket[8] = {0}; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length+6); + + if (rxpacket == NULL) + return result; + + if (id >= BROADCAST_ID) { + free(rxpacket); + return COMM_NOT_AVAILABLE; + } + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 4; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)address; + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)length; + + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS) { + if (error != 0) { + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + for (uint16_t s = 0; s < length; s++) { + data[s] = rxpacket[PKT_PARAMETER0 + s]; + } + // memcpy(data, &rxpacket[PKT_PARAMETER0], length); + } + + free(rxpacket); + // delete[] rxpacket; + return result; +} + +int Protocol1PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, + uint16_t address) { + return readTx(port, id, address, 1); +} +int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t id, + uint8_t *data, uint8_t *error) { + uint8_t data_read[1] = {0}; + int result = readRx(port, id, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; +} +int Protocol1PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint8_t *data, + uint8_t *error) { + uint8_t data_read[1] = {0}; + int result = readTxRx(port, id, address, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; +} + +int Protocol1PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, + uint16_t address) { + return readTx(port, id, address, 2); +} +int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint8_t id, + uint16_t *data, uint8_t *error) { + uint8_t data_read[2] = {0}; + int result = readRx(port, id, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; +} +int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t *data, + uint8_t *error) { + uint8_t data_read[2] = {0}; + int result = readTxRx(port, id, address, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; +} + +int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, + uint16_t address) { + return readTx(port, id, address, 4); +} +int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint8_t id, + uint32_t *data, uint8_t *error) { + uint8_t data_read[4] = {0}; + int result = 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 result; +} +int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint32_t *data, + uint8_t *error) { + uint8_t data_read[4] = {0}; + int result = 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 result; +} + +int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 7); + // uint8_t *txpacket = new uint8_t[length+7]; + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length + 3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 7); // #6->7 + // uint8_t *txpacket = new uint8_t[length+7]; + uint8_t rxpacket[6] = {0}; + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length + 3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + result = txRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint8_t data) { + uint8_t data_write[1] = {data}; + return writeTxOnly(port, id, address, 1, data_write); +} +int Protocol1PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint8_t data, + uint8_t *error) { + uint8_t data_write[1] = {data}; + return writeTxRx(port, id, address, 1, data_write, error); +} + +int Protocol1PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint16_t data) { + uint8_t data_write[2] = {DXL_LOBYTE(data), DXL_HIBYTE(data)}; + return writeTxOnly(port, id, address, 2, data_write); +} +int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t data, + uint8_t *error) { + uint8_t data_write[2] = {DXL_LOBYTE(data), DXL_HIBYTE(data)}; + return writeTxRx(port, id, address, 2, data_write, error); +} + +int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint32_t data) { + uint8_t data_write[4] = { + DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data))}; + return writeTxOnly(port, id, address, 4, data_write); +} +int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint32_t data, + uint8_t *error) { + uint8_t data_write[4] = { + DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data))}; + return writeTxRx(port, id, address, 4, data_write, error); +} + +int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 6); + // uint8_t *txpacket = new uint8_t[length+6]; + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length + 3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 6); + // uint8_t *txpacket = new uint8_t[length+6]; + uint8_t rxpacket[6] = {0}; + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length + 3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + + result = txRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::syncReadTx(PortHandler *port, + uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length) { + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, + uint16_t start_address, + uint16_t data_length, + uint8_t *param, + uint16_t param_length) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 8); + // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + // uint8_t *txpacket = new uint8_t[param_length + 8]; + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = + param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0 + 0] = start_address; + txpacket[PKT_PARAMETER0 + 1] = data_length; + + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); + + result = txRxPacket(port, txpacket, 0, 0); + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, + uint16_t param_length) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 7); + // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM + // uint8_t *txpacket = new uint8_t[param_length + 7]; + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; + txpacket[PKT_PARAMETER0 + 0] = 0x00; + + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); + + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) { + int wait_length = 0; + for (uint16_t i = 0; i < param_length; i += 3) + wait_length += param[i] + 7; + port->setPacketTimeout((uint16_t)wait_length); + } + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol1PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, + uint16_t param_length) { + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::fastSyncReadTx(PortHandler *port, + uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length) { + return COMM_NOT_AVAILABLE; +} + +int Protocol1PacketHandler::fastBulkReadTx(PortHandler *port, uint8_t *param, + uint16_t param_length) { + return COMM_NOT_AVAILABLE; +} \ No newline at end of file diff --git a/src/drivers/dynamixel/protocol1_packet_handler.h b/src/drivers/dynamixel/protocol1_packet_handler.h new file mode 100644 index 0000000..3c4b908 --- /dev/null +++ b/src/drivers/dynamixel/protocol1_packet_handler.h @@ -0,0 +1,671 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Protocol 1.0 Dynamixel packet control +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ + +#include "packet_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for control Dynamixel by using Protocol1.0 +//////////////////////////////////////////////////////////////////////////////// +class WINDECLSPEC Protocol1PacketHandler : public PacketHandler { +private: + static Protocol1PacketHandler *unique_instance_; + + Protocol1PacketHandler(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol1PacketHandler instance + /// @return Protocol1PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + static Protocol1PacketHandler *getInstance() { return unique_instance_; } + + virtual ~Protocol1PacketHandler() {} + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol version used in + /// Protocol1PacketHandler (1.0) + /// @return 1.0 + //////////////////////////////////////////////////////////////////////////////// + float getProtocolVersion() { return 1.0; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets description of communication result + /// @param result Communication result which might be gotten by the tx rx + /// functions + /// @return description of communication result in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + const char *getTxRxResult(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 + /// @return description of hardware error in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + const char *getRxPacketError(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, + /// @description then transmits txpacket by PortHandler::writePort() + /// function. + /// @description The function activates only when the port is not busy and + /// when the packet is already written on the port buffer + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @return COMM_PORT_BUSY + /// @return when the port is already in use + /// @return COMM_TX_ERROR + /// @return when txpacket is out of range described by TXPACKET_MAX_LEN + /// @return COMM_TX_FAIL + /// @return when written packet is shorter than expected + /// @return or COMM_SUCCESS + //////////////////////////////////////////////////////////////////////////////// + int txPacket(PortHandler *port, uint8_t *txpacket); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives packet (rxpacket) during designated time + /// via PortHandler port + /// @description The function repeatedly tries to receive rxpacket by + /// PortHandler::readPort() function. + /// @description It breaks out + /// @description when PortHandler::isPacketTimeout() shows the timeout, + /// @description when rxpacket seemed as corrupted, or + /// @description when nothing received + /// @param port PortHandler instance + /// @param rxpacket received packet + /// @return COMM_RX_CORRUPT + /// @return when it received the packet but it couldn't find header in the + /// packet + /// @return when it found header in the packet but the id, length or error + /// value is out of range + /// @return when it received the packet but it is shorted than expected + /// @return COMM_RX_TIMEOUT + /// @return when there is no rxpacket received until + /// PortHandler::isPacketTimeout() shows the timeout + /// @return COMM_SUCCESS + /// @return when rxpacket passes checksum test + /// @return or COMM_RX_FAIL + //////////////////////////////////////////////////////////////////////////////// + int rxPacket(PortHandler *port, uint8_t *rxpacket, + bool skip_stuffing = false); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits packet (txpacket) and receives packet + /// (rxpacket) during designated time via PortHandler port + /// @description The function calls Protocol1PacketHandler::txPacket(), + /// @description and calls Protocol1PacketHandler::rxPacket() if it succeeds + /// Protocol1PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it fails Protocol1PacketHandler::txPacket(), + /// @description when txpacket is called by + /// Protocol1PacketHandler::broadcastPing() / + /// Protocol1PacketHandler::syncWriteTxOnly() / + /// Protocol1PacketHandler::regWriteTxOnly / Protocol1PacketHandler::action + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @param rxpacket received packet + /// @return COMM_SUCCESS + /// @return when it succeeds Protocol1PacketHandler::txPacket() and + /// Protocol1PacketHandler::rxPacket() + /// @return or the other communication results which come from + /// Protocol1PacketHandler::txPacket() and Protocol1PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + int txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel but doesn't take its model number + /// @description The function calls Protocol1PacketHandler::ping() which gets + /// Dynamixel model number, + /// @description but doesn't carry the model number + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::ping() + //////////////////////////////////////////////////////////////////////////////// + int ping(PortHandler *port, uint8_t id, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel and takes its model number + /// @description The function makes an instruction packet with INST_PING, + /// @description transmits the packet with + /// Protocol1PacketHandler::txRxPacket(), + /// @description and call Protocol1PacketHandler::readTxRx to read + /// model_number in the rx buffer. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return COMM_SUCCESS + /// @return when it succeeds to ping Dynamixel and get model_number from it + /// @return or the other communication results which come from + /// Protocol1PacketHandler::txRxPacket() and + /// Protocol1PacketHandler::readTxRx() + //////////////////////////////////////////////////////////////////////////////// + int ping(PortHandler *port, uint8_t id, uint16_t *model_number, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that pings all + /// connected Dynamixel + /// @param port PortHandler instance + /// @param id_list ID list of Dynamixels which are found by broadcast ping + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int broadcastPing(PortHandler *port, std::vector &id_list); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixels run as written in the Dynamixel + /// register + /// @description The function makes an instruction packet with INST_ACTION, + /// @description transmits the packet with + /// Protocol1PacketHandler::txRxPacket(). + /// @description To use this function, Dynamixel register should be set by + /// Protocol1PacketHandler::regWriteTxOnly() or + /// Protocol1PacketHandler::regWriteTxRx() + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @return communication results which come from + /// Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int action(PortHandler *port, uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that makes Dynamixel + /// reboot + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int reboot(PortHandler *port, uint8_t id, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that reset multi-turn + /// revolution information of Dynamixel + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reset as it was produced in the + /// factory + /// @description The function makes an instruction packet with + /// INST_FACTORY_RESET, + /// @description transmits the packet with + /// Protocol1PacketHandler::txRxPacket(). + /// @description Be careful of the use. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param option (Not available in Protocol 1.0) Reset option + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int factoryReset(PortHandler *port, uint8_t id, uint8_t option, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits the packet with Protocol1PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from + /// Protocol1PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet and reads the data in the + /// packet + /// @description The function receives the packet which might be come by + /// previous INST_READ instruction packet transmission, + /// @description gets the data from the packet. + /// @param port PortHandler instance + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + int readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet, and read + /// data from received packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits and receives the packet with + /// Protocol1PacketHandler::txRxPacket(), + /// @description gets the data from the packet. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from + /// Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTx() function + /// for reading 1 byte data + /// @description The function calls Protocol1PacketHandler::readTx() function + /// for reading 1 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from + /// Protocol1PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteTx(PortHandler *port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readRx() function + /// and reads 1 byte data on the packet + /// @description The function calls Protocol1PacketHandler::readRx() function, + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTxRx() function + /// for reading 1 byte data + /// @description The function calls Protocol1PacketHandler::readTxRx(), + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint8_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTx() function + /// for reading 2 byte data + /// @description The function calls Protocol1PacketHandler::readTx() function + /// for reading 2 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from + /// Protocol1PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteTx(PortHandler *port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readRx() function + /// and reads 2 byte data on the packet + /// @description The function calls Protocol1PacketHandler::readRx() function, + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTxRx() function + /// for reading 2 byte data + /// @description The function calls Protocol1PacketHandler::readTxRx(), + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTx() function + /// for reading 4 byte data + /// @description The function calls Protocol1PacketHandler::readTx() function + /// for reading 4 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from + /// Protocol1PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteTx(PortHandler *port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readRx() function + /// and reads 4 byte data on the packet + /// @description The function calls Protocol1PacketHandler::readRx() function, + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTxRx() function + /// for reading 4 byte data + /// @description The function calls Protocol1PacketHandler::readTxRx(), + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint32_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the + /// data for write + /// @description The function makes an instruction packet with INST_WRITE and + /// the data for write, + /// @description transmits the packet with Protocol1PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol1PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the + /// data for write, and receives the packet + /// @description The function makes an instruction packet with INST_WRITE and + /// the data for write, + /// @description transmits and receives the packet with + /// Protocol1PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int writeTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxOnly() for + /// writing 1 byte data + /// @description The function calls Protocol1PacketHandler::writeTxOnly() for + /// writing 1 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol1PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint8_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxRx() for + /// writing 1 byte data and receives the packet + /// @description The function calls Protocol1PacketHandler::writeTxRx() for + /// writing 1 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint8_t data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxOnly() for + /// writing 2 byte data + /// @description The function calls Protocol1PacketHandler::writeTxOnly() for + /// writing 2 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol1PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint16_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxRx() for + /// writing 2 byte data and receives the packet + /// @description The function calls Protocol1PacketHandler::writeTxRx() for + /// writing 2 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxOnly() for + /// writing 4 byte data + /// @description The function calls Protocol1PacketHandler::writeTxOnly() for + /// writing 4 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol1PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint32_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxRx() for + /// writing 4 byte data and receives the packet + /// @description The function calls Protocol1PacketHandler::writeTxRx() for + /// writing 4 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint32_t data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with + /// the data for writing on the Dynamixel register + /// @description The function makes an instruction packet with INST_REG_WRITE + /// and the data for writing on the Dynamixel register, + /// @description transmits the packet with Protocol1PacketHandler::txPacket(). + /// @description The data written in the register will act when INST_ACTION + /// instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol1PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with + /// the data for writing on the Dynamixel register, and receives the packet + /// @description The function makes an instruction packet with INST_REG_WRITE + /// and the data for writing on the Dynamixel register, + /// @description transmits and receives the packet with + /// Protocol1PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @description The data written in the register will act when INST_ACTION + /// instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that transmits Sync + /// Read instruction packet + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Read + /// @param data_length Length of the data for Sync Read + /// @param param Parameter for Sync Read + /// @param param_length Length of the data for Sync Read + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int syncReadTx(PortHandler *port, uint16_t start_address, + uint16_t data_length, uint8_t *param, uint16_t param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits Sync Write instruction packet + /// @description The function makes an instruction packet with + /// INST_SYNC_WRITE, + /// @description transmits the packet with + /// Protocol1PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Write + /// @param data_length Length of the data for Sync Write + /// @param param Parameter for Sync Write {ID1, DATA0, DATA1, ..., DATAn, ID2, + /// DATA0, DATA1, ..., DATAn, ID3, DATA0, DATA1, ..., DATAn} + /// @param param_length Length of the data for Sync Write + /// @return communication results which come from + /// Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int syncWriteTxOnly(PortHandler *port, uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only on Dynamixel MX / X series) The function that + /// transmits Bulk Read instruction packet + /// @description The function makes an instruction packet with INST_BULK_READ, + /// @description transmits the packet with Protocol1PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Read {LEN1, ID1, ADDR1, LEN2, ID2, ADDR2, + /// ...} + /// @param param_length Length of the data for Bulk Read + /// @return communication results which come from + /// Protocol1PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that transmits Bulk + /// Write instruction packet + /// @param port PortHandler instance + /// @param param Parameter for Bulk Write + /// @param param_length Length of the data for Bulk Write + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length); + + int fastSyncReadTx(PortHandler *port, uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length); + int fastBulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ \ No newline at end of file diff --git a/src/drivers/dynamixel/protocol2_packet_handler.cpp b/src/drivers/dynamixel/protocol2_packet_handler.cpp new file mode 100644 index 0000000..56e2240 --- /dev/null +++ b/src/drivers/dynamixel/protocol2_packet_handler.cpp @@ -0,0 +1,1151 @@ +/******************************************************************************* + * 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: zerom, Ryu Woon Jung (Leon) */ + +#if defined(__linux__) +#include "protocol2_packet_handler.h" +#include +#elif defined(__APPLE__) +#include "protocol2_packet_handler.h" +#include +#elif defined(_WIN32) || defined(_WIN64) +#define WINDLLEXPORT +#include "protocol2_packet_handler.h" +#include +#elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) || \ + defined(ARDUINO_OpenRB) +#include "../../include/dynamixel_sdk/protocol2_packet_handler.h" +#endif + +#include +#include +#include + +#define TXPACKET_MAX_LEN (1 * 1024) +#define RXPACKET_MAX_LEN (1 * 1024) + +///////////////// for Protocol 2.0 Packet ///////////////// +#define PKT_HEADER0 0 +#define PKT_HEADER1 1 +#define PKT_HEADER2 2 +#define PKT_RESERVED 3 +#define PKT_ID 4 +#define PKT_LENGTH_L 5 +#define PKT_LENGTH_H 6 +#define PKT_INSTRUCTION 7 +#define PKT_ERROR 8 +#define PKT_PARAMETER0 8 + +///////////////// Protocol 2.0 Error bit ///////////////// +#define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet. +#define ERRNUM_INSTRUCTION 2 // Instruction error +#define ERRNUM_CRC 3 // CRC check error +#define ERRNUM_DATA_RANGE 4 // Data range error +#define ERRNUM_DATA_LENGTH 5 // Data length error +#define ERRNUM_DATA_LIMIT 6 // Data limit error +#define ERRNUM_ACCESS 7 // Access error + +#define ERRBIT_ALERT \ + 128 // When the device has a problem, this bit is set to 1. Check "Device + // Status Check" value. + +using namespace dynamixel; + +Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = + new Protocol2PacketHandler(); + +Protocol2PacketHandler::Protocol2PacketHandler() {} + +const char *Protocol2PacketHandler::getTxRxResult(int result) { + switch (result) { + case COMM_SUCCESS: + return "[TxRxResult] Communication success."; + + case COMM_PORT_BUSY: + return "[TxRxResult] Port is in use!"; + + case COMM_TX_FAIL: + return "[TxRxResult] Failed transmit instruction packet!"; + + case COMM_RX_FAIL: + return "[TxRxResult] Failed get status packet from device!"; + + case COMM_TX_ERROR: + return "[TxRxResult] Incorrect instruction packet!"; + + case COMM_RX_WAITING: + return "[TxRxResult] Now recieving status packet!"; + + case COMM_RX_TIMEOUT: + return "[TxRxResult] There is no status packet!"; + + case COMM_RX_CORRUPT: + return "[TxRxResult] Incorrect status packet!"; + + case COMM_NOT_AVAILABLE: + return "[TxRxResult] Protocol does not support This function!"; + + default: + return ""; + } +} + +const char *Protocol2PacketHandler::getRxPacketError(uint8_t error) { + if (error & ERRBIT_ALERT) + return "[RxPacketError] Hardware error occurred. Check the error at " + "Control Table (Hardware Error Status)!"; + + int not_alert_error = error & ~ERRBIT_ALERT; + + switch (not_alert_error) { + case 0: + return ""; + + case ERRNUM_RESULT_FAIL: + return "[RxPacketError] Failed to process the instruction packet!"; + + case ERRNUM_INSTRUCTION: + return "[RxPacketError] Undefined instruction or incorrect instruction!"; + + case ERRNUM_CRC: + return "[RxPacketError] CRC doesn't match!"; + + case ERRNUM_DATA_RANGE: + return "[RxPacketError] The data value is out of range!"; + + case ERRNUM_DATA_LENGTH: + return "[RxPacketError] The data length does not match as expected!"; + + case ERRNUM_DATA_LIMIT: + return "[RxPacketError] The data value exceeds the limit value!"; + + case ERRNUM_ACCESS: + return "[RxPacketError] Writing or Reading is not available to target " + "address!"; + + default: + return "[RxPacketError] Unknown error code!"; + } +} + +unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, + uint8_t *data_blk_ptr, + uint16_t data_blk_size) { + uint16_t i; + static const uint16_t crc_table[256] = { + 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x8033, + 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8063, 0x0066, + 0x006C, 0x8069, 0x0078, 0x807D, 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 (uint16_t j = 0; j < data_blk_size; j++) { + i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; + crc_accum = (crc_accum << 8) ^ crc_table[i]; + } + + return crc_accum; +} + +void Protocol2PacketHandler::addStuffing(uint8_t *packet) { + int packet_length_in = + DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; + + if (packet_length_in < + 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD + return; + + uint8_t *packet_ptr; + uint16_t packet_length_before_crc = packet_length_in - 2; + for (uint16_t i = 3; i < packet_length_before_crc; i++) { + packet_ptr = &packet[i + PKT_INSTRUCTION - 2]; + if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) + packet_length_out++; + } + + if (packet_length_in == packet_length_out) // no stuffing required + return; + + uint16_t out_index = packet_length_out + 6 - 2; // last index before crc + uint16_t in_index = packet_length_in + 6 - 2; // last index before crc + while (out_index != in_index) { + if (packet[in_index] == 0xFD && packet[in_index - 1] == 0xFF && + packet[in_index - 2] == 0xFF) { + packet[out_index--] = 0xFD; // byte stuffing + if (out_index != in_index) { + packet[out_index--] = packet[in_index--]; // FD + packet[out_index--] = packet[in_index--]; // FF + packet[out_index--] = packet[in_index--]; // FF + } + } else { + packet[out_index--] = packet[in_index--]; + } + } + + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + + return; +} + +void Protocol2PacketHandler::removeStuffing(uint8_t *packet) { + int i = 0, index = 0; + int packet_length_in = + DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; + + index = PKT_INSTRUCTION; + for (i = 0; i < packet_length_in - 2; i++) // except CRC + { + if (packet[i + PKT_INSTRUCTION] == 0xFD && + packet[i + PKT_INSTRUCTION + 1] == 0xFD && + packet[i + PKT_INSTRUCTION - 1] == 0xFF && + packet[i + PKT_INSTRUCTION - 2] == 0xFF) { // FF FF FD FD + packet_length_out--; + i++; + } + packet[index++] = packet[i + PKT_INSTRUCTION]; + } + packet[index++] = packet[PKT_INSTRUCTION + packet_length_in - 2]; + packet[index++] = 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); +} + +int Protocol2PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) { + uint16_t total_packet_length = 0; + uint16_t written_packet_length = 0; + + if (port->is_using_) + return COMM_PORT_BUSY; + port->is_using_ = true; + + // byte stuffing for header + 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 + uint16_t crc = 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, total_packet_length); + if (total_packet_length != written_packet_length) { + port->is_using_ = false; + return COMM_TX_FAIL; + } + + return COMM_SUCCESS; +} + +int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket, + bool skip_stuffing) { + int result = COMM_TX_FAIL; + + uint16_t rx_length = 0; + uint16_t wait_length = 11; // minimum length (HEADER0 HEADER1 HEADER2 RESERVED + // ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H) + + while (true) { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (rx_length >= wait_length) { + uint16_t idx = 0; + + // find packet header + for (idx = 0; idx < (rx_length - 3); idx++) { + if ((rxpacket[idx] == 0xFF) && (rxpacket[idx + 1] == 0xFF) && + (rxpacket[idx + 2] == 0xFD) && (rxpacket[idx + 3] != 0xFD)) + break; + } + + if (idx == 0) // found at the beginning of the packet + { + if (rxpacket[PKT_RESERVED] != 0x00 || + // rxpacket[PKT_ID] > 0xFC || // FAST protocol responds + // with a broadcast ID + DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > + RXPACKET_MAX_LEN || + rxpacket[PKT_INSTRUCTION] != 0x55) { + // remove the first byte in the packet + for (uint16_t s = 0; s < rx_length - 1; s++) + rxpacket[s] = rxpacket[1 + s]; + // memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= 1; + continue; + } + + // re-calculate the exact length of the rx packet + 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) { + // check timeout + if (port->isPacketTimeout() == true) { + if (rx_length == 0) { + result = COMM_RX_TIMEOUT; + } else { + result = COMM_RX_CORRUPT; + } + break; + } else { + continue; + } + } + + // verify CRC16 + uint16_t crc = + DXL_MAKEWORD(rxpacket[wait_length - 2], rxpacket[wait_length - 1]); + if (updateCRC(0, rxpacket, wait_length - 2) == crc) { + result = COMM_SUCCESS; + } else { + result = COMM_RX_CORRUPT; + } + break; + } else { + // remove unnecessary packets + for (uint16_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + // memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= idx; + } + } else { + // check timeout + if (port->isPacketTimeout() == true) { + if (rx_length == 0) { + result = COMM_RX_TIMEOUT; + } else { + result = COMM_RX_CORRUPT; + } + break; + } + } +#if defined(__linux__) || defined(__APPLE__) + usleep(0); +#elif defined(_WIN32) || defined(_WIN64) + Sleep(0); +#endif + } + port->is_using_ = false; + + if ((result == COMM_SUCCESS) && (false == skip_stuffing)) + removeStuffing(rxpacket); + + return result; +} + +// NOT for BulkRead / SyncRead instruction +int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, + uint8_t *rxpacket, uint8_t *error) { + int result = COMM_TX_FAIL; + + // tx packet + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + return result; + + // (Instruction == BulkRead or SyncRead) == this function is not available. + if (txpacket[PKT_INSTRUCTION] == INST_BULK_READ || + 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 || + txpacket[PKT_INSTRUCTION] == INST_ACTION) { + port->is_using_ = false; + return result; + } + + // set packet timeout + if (txpacket[PKT_INSTRUCTION] == INST_READ) { + port->setPacketTimeout( + (uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0 + 2], + txpacket[PKT_PARAMETER0 + 3]) + + 11)); + } else { + port->setPacketTimeout((uint16_t)11); + // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L + // CRC16_H + } + + // rx packet + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]); + + if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + + return result; +} + +int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, + uint8_t *error) { + return ping(port, id, 0, error); +} + +int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, + uint16_t *model_number, uint8_t *error) { + int result = COMM_TX_FAIL; + + uint8_t txpacket[10] = {0}; + uint8_t rxpacket[14] = {0}; + + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; + + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS && model_number != 0) + *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], + rxpacket[PKT_PARAMETER0 + 2]); + + return result; +} + +int Protocol2PacketHandler::broadcastPing(PortHandler *port, + std::vector &id_list) { + const int STATUS_LENGTH = 14; + int result = COMM_TX_FAIL; + + id_list.clear(); + + uint16_t rx_length = 0; + uint16_t wait_length = STATUS_LENGTH * MAX_ID; + + uint8_t txpacket[10] = {0}; + uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0}; + + double tx_time_per_byte = (1000.0 / (double)port->getBaudRate()) * 10.0; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; + + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) { + port->is_using_ = false; + return result; + } + + // set rx timeout + // port->setPacketTimeout((uint16_t)(wait_length * 30)); + port->setPacketTimeout(((double)wait_length * tx_time_per_byte) + + (3.0 * (double)MAX_ID) + 16.0); + + while (1) { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (port->isPacketTimeout() == true) // || rx_length >= wait_length) + break; + } + + port->is_using_ = false; + + if (rx_length == 0) + return COMM_RX_TIMEOUT; + + while (1) { + if (rx_length < STATUS_LENGTH) + return COMM_RX_CORRUPT; + + uint16_t idx = 0; + + // find packet header + for (idx = 0; idx < (rx_length - 2); idx++) { + if (rxpacket[idx] == 0xFF && rxpacket[idx + 1] == 0xFF && + rxpacket[idx + 2] == 0xFD) + break; + } + + if (idx == 0) // found at the beginning of the packet + { + // verify CRC16 + uint16_t crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH - 2], + rxpacket[STATUS_LENGTH - 1]); + + if (updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) { + result = COMM_SUCCESS; + + id_list.push_back(rxpacket[PKT_ID]); + + for (uint16_t s = 0; s < rx_length - STATUS_LENGTH; s++) + rxpacket[s] = rxpacket[STATUS_LENGTH + s]; + rx_length -= STATUS_LENGTH; + + if (rx_length == 0) + return result; + } else { + result = COMM_RX_CORRUPT; + + // remove header (0xFF 0xFF 0xFD) + for (uint16_t s = 0; s < rx_length - 3; s++) + rxpacket[s] = rxpacket[3 + s]; + rx_length -= 3; + } + } else { + // remove unnecessary packets + for (uint16_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + rx_length -= idx; + } + } + + return result; +} + +int Protocol2PacketHandler::action(PortHandler *port, uint8_t id) { + uint8_t txpacket[10] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_ACTION; + + return txRxPacket(port, txpacket, 0); +} + +int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, + uint8_t *error) { + uint8_t txpacket[10] = {0}; + uint8_t rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_REBOOT; + + return txRxPacket(port, txpacket, rxpacket, error); +} + +int Protocol2PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, + uint8_t *error) { + uint8_t txpacket[15] = {0}; + uint8_t rxpacket[11] = {0}; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 8; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_CLEAR; + txpacket[PKT_PARAMETER0] = 0x01; + txpacket[PKT_PARAMETER0 + 1] = 0x44; + txpacket[PKT_PARAMETER0 + 2] = 0x58; + txpacket[PKT_PARAMETER0 + 3] = 0x4C; + txpacket[PKT_PARAMETER0 + 4] = 0x22; + + return txRxPacket(port, txpacket, rxpacket, error); +} + +int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, + uint8_t option, uint8_t *error) { + uint8_t txpacket[11] = {0}; + uint8_t rxpacket[11] = {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; + + return txRxPacket(port, txpacket, rxpacket, error); +} + +int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length) { + int result = COMM_TX_FAIL; + + uint8_t txpacket[14] = {0}; + + 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] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0 + 2] = (uint8_t)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0 + 3] = (uint8_t)DXL_HIBYTE(length); + + result = txPacket(port, txpacket); + + // set packet timeout + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)(length + 11)); + + return result; +} + +int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, + uint16_t length, uint8_t *data, + uint8_t *error) { + int result = COMM_TX_FAIL; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + //(length + 11 + (length/3)); // (length/3): consider stuffing + + if (rxpacket == NULL) + return result; + + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); + + if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + + for (uint16_t s = 0; s < length; s++) { + data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + } + // memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); + } + + free(rxpacket); + // delete[] rxpacket; + return result; +} + +int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error) { + int result = COMM_TX_FAIL; + + uint8_t txpacket[14] = {0}; + uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); + //(length + 11 + (length/3)); // (length/3): consider stuffing + + if (rxpacket == NULL) + return result; + + if (id >= BROADCAST_ID) { + free(rxpacket); + return COMM_NOT_AVAILABLE; + } + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 7; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0 + 2] = (uint8_t)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0 + 3] = (uint8_t)DXL_HIBYTE(length); + + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS) { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + + for (uint16_t s = 0; s < length; s++) { + data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + } + // memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); + } + + free(rxpacket); + // delete[] rxpacket; + return result; +} + +int Protocol2PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, + uint16_t address) { + return readTx(port, id, address, 1); +} +int Protocol2PacketHandler::read1ByteRx(PortHandler *port, uint8_t id, + uint8_t *data, uint8_t *error) { + uint8_t data_read[1] = {0}; + int result = readRx(port, id, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; +} +int Protocol2PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint8_t *data, + uint8_t *error) { + uint8_t data_read[1] = {0}; + int result = readTxRx(port, id, address, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; +} + +int Protocol2PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, + uint16_t address) { + return readTx(port, id, address, 2); +} +int Protocol2PacketHandler::read2ByteRx(PortHandler *port, uint8_t id, + uint16_t *data, uint8_t *error) { + uint8_t data_read[2] = {0}; + int result = readRx(port, id, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; +} +int Protocol2PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t *data, + uint8_t *error) { + uint8_t data_read[2] = {0}; + int result = readTxRx(port, id, address, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; +} + +int Protocol2PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, + uint16_t address) { + return readTx(port, id, address, 4); +} +int Protocol2PacketHandler::read4ByteRx(PortHandler *port, uint8_t id, + uint32_t *data, uint8_t *error) { + uint8_t data_read[4] = {0}; + int result = 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 result; +} +int Protocol2PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint32_t *data, + uint8_t *error) { + uint8_t data_read[4] = {0}; + int result = 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 result; +} + +int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + uint8_t rxpacket[11] = {0}; + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol2PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint8_t data) { + uint8_t data_write[1] = {data}; + return writeTxOnly(port, id, address, 1, data_write); +} +int Protocol2PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint8_t data, + uint8_t *error) { + uint8_t data_write[1] = {data}; + return writeTxRx(port, id, address, 1, data_write, error); +} + +int Protocol2PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint16_t data) { + uint8_t data_write[2] = {DXL_LOBYTE(data), DXL_HIBYTE(data)}; + return writeTxOnly(port, id, address, 2, data_write); +} +int Protocol2PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t data, + uint8_t *error) { + uint8_t data_write[2] = {DXL_LOBYTE(data), DXL_HIBYTE(data)}; + return writeTxRx(port, id, address, 2, data_write, error); +} + +int Protocol2PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint32_t data) { + uint8_t data_write[4] = { + DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data))}; + return writeTxOnly(port, id, address, 4, data_write); +} +int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint32_t data, + uint8_t *error) { + uint8_t data_write[4] = { + DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), + DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data))}; + return writeTxRx(port, id, address, 4, data_write, error); +} + +int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, + uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + uint8_t rxpacket[11] = {0}; + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol2PacketHandler::syncReadTx(PortHandler *port, + uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L + // START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = + DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H + // DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = + DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H + // DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + 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); + + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + 4 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); + + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); + + free(txpacket); + return result; +} + +int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, + uint16_t start_address, + uint16_t data_length, + uint8_t *param, + uint16_t param_length) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L + // START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = + DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H + // DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = + DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H + // DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + 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); + + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + 4 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); + + result = txRxPacket(port, txpacket, 0, 0); + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, + uint16_t param_length) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = + DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = + DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; + + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0], param, param_length); + + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) { + int wait_length = 0; + for (uint16_t i = 0; i < param_length; i += 5) + wait_length += DXL_MAKEWORD(param[i + 3], param[i + 4]) + 10; + port->setPacketTimeout((uint16_t)wait_length); + } + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, + uint16_t param_length) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = + DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = + DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; + + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0], param, param_length); + + result = txRxPacket(port, txpacket, 0, 0); + + free(txpacket); + // delete[] txpacket; + return result; +} + +int Protocol2PacketHandler::fastSyncReadTx(PortHandler *port, + uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L + // START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + if (NULL == txpacket) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = + DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H + // DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = + DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H + // DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_FAST_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); + + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + 4 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); + + result = txPacket(port, txpacket); + if (COMM_SUCCESS == result) + port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); + + free(txpacket); + return result; +} + +int Protocol2PacketHandler::fastBulkReadTx(PortHandler *port, uint8_t *param, + uint16_t param_length) { + int result = COMM_TX_FAIL; + + uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + + if (NULL == txpacket) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = + DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = + DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_FAST_BULK_READ; + + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0], param, param_length); + + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) { + int wait_length = 0; + for (uint16_t i = 0; i < param_length; i += 5) + wait_length += DXL_MAKEWORD(param[i + 3], param[i + 4]) + 10; + port->setPacketTimeout((uint16_t)wait_length); + } + + free(txpacket); + // delete[] txpacket; + return result; +} \ No newline at end of file diff --git a/src/drivers/dynamixel/protocol2_packet_handler.h b/src/drivers/dynamixel/protocol2_packet_handler.h new file mode 100644 index 0000000..d86fa06 --- /dev/null +++ b/src/drivers/dynamixel/protocol2_packet_handler.h @@ -0,0 +1,695 @@ +/******************************************************************************* + * 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. + *******************************************************************************/ + +//////////////////////////////////////////////////////////////////////////////// +/// @file The file for Protocol 2.0 Dynamixel packet control +/// @author Zerom, Leon (RyuWoon Jung) +//////////////////////////////////////////////////////////////////////////////// + +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ + +#include "packet_handler.h" + +namespace dynamixel { + +//////////////////////////////////////////////////////////////////////////////// +/// @brief The class for control Dynamixel by using Protocol2.0 +//////////////////////////////////////////////////////////////////////////////// +class WINDECLSPEC Protocol2PacketHandler : public PacketHandler { +private: + static Protocol2PacketHandler *unique_instance_; + + Protocol2PacketHandler(); + + uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, + uint16_t data_blk_size); + void addStuffing(uint8_t *packet); + void removeStuffing(uint8_t *packet); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol2PacketHandler instance + /// @return Protocol2PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + static Protocol2PacketHandler *getInstance() { return unique_instance_; } + + virtual ~Protocol2PacketHandler() {} + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol version used in + /// Protocol2PacketHandler (2.0) + /// @return 2.0 + //////////////////////////////////////////////////////////////////////////////// + float getProtocolVersion() { return 2.0; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets description of communication result + /// @param result Communication result which might be gotten by the tx rx + /// functions + /// @return description of communication result in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + const char *getTxRxResult(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 + /// @return description of hardware error in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + const char *getRxPacketError(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, + /// @description then transmits txpacket by PortHandler::writePort() + /// function. + /// @description The function activates only when the port is not busy and + /// when the packet is already written on the port buffer + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @return COMM_PORT_BUSY + /// @return when the port is already in use + /// @return COMM_TX_ERROR + /// @return when txpacket is out of range described by TXPACKET_MAX_LEN + /// @return COMM_TX_FAIL + /// @return when written packet is shorter than expected + /// @return or COMM_SUCCESS + //////////////////////////////////////////////////////////////////////////////// + int txPacket(PortHandler *port, uint8_t *txpacket); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives packet (rxpacket) during designated time + /// via PortHandler port + /// @description The function repeatedly tries to receive rxpacket by + /// PortHandler::readPort() function. + /// @description It breaks out + /// @description when PortHandler::isPacketTimeout() shows the timeout, + /// @description when rxpacket seemed as corrupted, or + /// @description when nothing received + /// @param port PortHandler instance + /// @param rxpacket received packet + /// @return COMM_RX_CORRUPT + /// @return when it received the packet but it couldn't find header in the + /// packet + /// @return when it found header in the packet but the id, length or error + /// value is out of range + /// @return when it received the packet but it is shorted than expected + /// @return COMM_RX_TIMEOUT + /// @return when there is no rxpacket received until + /// PortHandler::isPacketTimeout() shows the timeout + /// @return COMM_SUCCESS + /// @return when rxpacket passes checksum test + /// @return or COMM_RX_FAIL + //////////////////////////////////////////////////////////////////////////////// + int rxPacket(PortHandler *port, uint8_t *rxpacket, + bool skip_stuffing = false); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits packet (txpacket) and receives packet + /// (rxpacket) during designated time via PortHandler port + /// @description The function calls Protocol2PacketHandler::txPacket(), + /// @description and calls Protocol2PacketHandler::rxPacket() if it succeeds + /// Protocol2PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it fails Protocol2PacketHandler::txPacket(), + /// @description when txpacket is called by + /// Protocol2PacketHandler::broadcastPing() / + /// Protocol2PacketHandler::syncWriteTxOnly() / + /// Protocol2PacketHandler::regWriteTxOnly / Protocol2PacketHandler::action + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @param rxpacket received packet + /// @return COMM_SUCCESS + /// @return when it succeeds Protocol2PacketHandler::txPacket() and + /// Protocol2PacketHandler::rxPacket() + /// @return or the other communication results which come from + /// Protocol2PacketHandler::txPacket() and Protocol2PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + int txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel but doesn't take its model number + /// @description The function calls Protocol2PacketHandler::ping() which gets + /// Dynamixel model number, + /// @description but doesn't carry the model number + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::ping() + //////////////////////////////////////////////////////////////////////////////// + int ping(PortHandler *port, uint8_t id, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel and takes its model number + /// @description The function makes an instruction packet with INST_PING, + /// @description transmits the packet with + /// Protocol2PacketHandler::txRxPacket(), + /// @description and call Protocol2PacketHandler::readTxRx to read + /// model_number in the rx buffer. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return COMM_SUCCESS + /// @return when it succeeds to ping Dynamixel and get model_number from it + /// @return or the other communication results which come from + /// Protocol2PacketHandler::txRxPacket() and + /// Protocol2PacketHandler::readTxRx() + //////////////////////////////////////////////////////////////////////////////// + int ping(PortHandler *port, uint8_t id, uint16_t *model_number, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that pings all + /// connected Dynamixel + /// @param port PortHandler instance + /// @param id_list ID list of Dynamixels which are found by broadcast ping + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int broadcastPing(PortHandler *port, std::vector &id_list); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixels run as written in the Dynamixel + /// register + /// @description The function makes an instruction packet with INST_ACTION, + /// @description transmits the packet with + /// Protocol2PacketHandler::txRxPacket(). + /// @description To use this function, Dynamixel register should be set by + /// Protocol2PacketHandler::regWriteTxOnly() or + /// Protocol2PacketHandler::regWriteTxRx() + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int action(PortHandler *port, uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reboot + /// @description The function makes an instruction packet with INST_REBOOT, + /// @description transmits the packet with + /// Protocol2PacketHandler::txRxPacket(), + /// @description then Dynamixel reboots. + /// @description During reboot, its LED will blink. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int reboot(PortHandler *port, uint8_t id, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reset multi-turn revolution information of + /// Dynamixel + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with + /// Protocol2PacketHandler::txRxPacket(). + /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or + /// above), + /// @description Dynamixel X-series (Firmware v42 or above). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reset as it was produced in the + /// factory + /// @description The function makes an instruction packet with + /// INST_FACTORY_RESET, + /// @description transmits the packet with + /// Protocol2PacketHandler::txRxPacket(). + /// @description Be careful of the use. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param option Reset option (0xFF for reset all values / 0x01 for reset all + /// values except ID / 0x02 for reset all values except ID and Baudrate) + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int factoryReset(PortHandler *port, uint8_t id, uint8_t option, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from + /// Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet and reads the data in the + /// packet + /// @description The function receives the packet which might be come by + /// previous INST_READ instruction packet transmission, + /// @description gets the data from the packet. + /// @param port PortHandler instance + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + int readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet, and read + /// data from received packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits and receives the packet with + /// Protocol2PacketHandler::txRxPacket(), + /// @description gets the data from the packet. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, + uint8_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTx() function + /// for reading 1 byte data + /// @description The function calls Protocol2PacketHandler::readTx() function + /// for reading 1 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from + /// Protocol2PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteTx(PortHandler *port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readRx() function + /// and reads 1 byte data on the packet + /// @description The function calls Protocol2PacketHandler::readRx() function, + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTxRx() function + /// for reading 1 byte data + /// @description The function calls Protocol2PacketHandler::readTxRx(), + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint8_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTx() function + /// for reading 2 byte data + /// @description The function calls Protocol2PacketHandler::readTx() function + /// for reading 2 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from + /// Protocol2PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteTx(PortHandler *port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readRx() function + /// and reads 2 byte data on the packet + /// @description The function calls Protocol2PacketHandler::readRx() function, + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTxRx() function + /// for reading 2 byte data + /// @description The function calls Protocol2PacketHandler::readTxRx(), + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTx() function + /// for reading 4 byte data + /// @description The function calls Protocol2PacketHandler::readTx() function + /// for reading 4 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from + /// Protocol2PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteTx(PortHandler *port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readRx() function + /// and reads 4 byte data on the packet + /// @description The function calls Protocol2PacketHandler::readRx() function, + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, + uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTxRx() function + /// for reading 4 byte data + /// @description The function calls Protocol2PacketHandler::readTxRx(), + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint32_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the + /// data for write + /// @description The function makes an instruction packet with INST_WRITE and + /// the data for write, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the + /// data for write, and receives the packet + /// @description The function makes an instruction packet with INST_WRITE and + /// the data for write, + /// @description transmits and receives the packet with + /// Protocol2PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int writeTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxOnly() for + /// writing 1 byte data + /// @description The function calls Protocol2PacketHandler::writeTxOnly() for + /// writing 1 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol2PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint8_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxRx() for + /// writing 1 byte data and receives the packet + /// @description The function calls Protocol2PacketHandler::writeTxRx() for + /// writing 1 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint8_t data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxOnly() for + /// writing 2 byte data + /// @description The function calls Protocol2PacketHandler::writeTxOnly() for + /// writing 2 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol2PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint16_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxRx() for + /// writing 2 byte data and receives the packet + /// @description The function calls Protocol2PacketHandler::writeTxRx() for + /// writing 2 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxOnly() for + /// writing 4 byte data + /// @description The function calls Protocol2PacketHandler::writeTxOnly() for + /// writing 4 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol2PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint32_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxRx() for + /// writing 4 byte data and receives the packet + /// @description The function calls Protocol2PacketHandler::writeTxRx() for + /// writing 4 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint32_t data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with + /// the data for writing on the Dynamixel register + /// @description The function makes an instruction packet with INST_REG_WRITE + /// and the data for writing on the Dynamixel register, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @description The data written in the register will act when INST_ACTION + /// instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from + /// Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with + /// the data for writing on the Dynamixel register, and receives the packet + /// @description The function makes an instruction packet with INST_REG_WRITE + /// and the data for writing on the Dynamixel register, + /// @description transmits and receives the packet with + /// Protocol2PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @description The data written in the register will act when INST_ACTION + /// instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, + uint16_t length, uint8_t *data, uint8_t *error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_SYNC_READ instruction packet + /// @description The function makes an instruction packet with INST_SYNC_READ, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Read + /// @param data_length Length of the data for Sync Read + /// @param param Parameter for Sync Read + /// @param param_length Length of the data for Sync Read + /// @return communication results which come from + /// Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int syncReadTx(PortHandler *port, uint16_t start_address, + uint16_t data_length, uint8_t *param, uint16_t param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_SYNC_WRITE instruction packet + /// @description The function makes an instruction packet with + /// INST_SYNC_WRITE, + /// @description transmits the packet with + /// Protocol2PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Write + /// @param data_length Length of the data for Sync Write + /// @param param Parameter for Sync Write {ID1, DATA0, DATA1, ..., DATAn, ID2, + /// DATA0, DATA1, ..., DATAn, ID3, DATA0, DATA1, ..., DATAn} + /// @param param_length Length of the data for Sync Write + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int syncWriteTxOnly(PortHandler *port, uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_BULK_READ instruction packet + /// @description The function makes an instruction packet with INST_BULK_READ, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Read {ID1, ADDR_L1, ADDR_H1, LEN_L1, + /// LEN_H1, ID2, ADDR_L2, ADDR_H2, LEN_L2, LEN_H2, ...} + /// @param param_length Length of the data for Bulk Read + /// @return communication results which come from + /// Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_BULK_WRITE instruction packet + /// @description The function makes an instruction packet with + /// INST_BULK_WRITE, + /// @description transmits the packet with + /// Protocol2PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Write {ID1, START_ADDR_L, START_ADDR_H, + /// DATA_LEN_L, DATA_LEN_H, DATA0, DATA1, ..., DATAn, ID2, START_ADDR_L, + /// START_ADDR_H, DATA_LEN_L, DATA_LEN_H, DATA0, DATA1, ..., DATAn} + /// @param param_length Length of the data for Bulk Write + /// @return communication results which come from + /// Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length); + + int fastSyncReadTx(PortHandler *port, uint16_t start_address, + uint16_t data_length, uint8_t *param, + uint16_t param_length); + int fastBulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length); +}; + +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ \ No newline at end of file