diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt deleted file mode 100644 index 658dfd4..0000000 --- a/dynamixel_sdk/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(dynamixel_sdk) - -find_package(catkin REQUIRED COMPONENTS - roscpp -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES dynamixel_sdk -# CATKIN_DEPENDS roscpp -# DEPENDS system_lib -) - -# include_directories(include) -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(dynamixel_sdk - src/${PROJECT_NAME}/packet_handler.cpp - src/${PROJECT_NAME}/protocol1_packet_handler.cpp - src/${PROJECT_NAME}/protocol2_packet_handler.cpp - src/${PROJECT_NAME}/group_sync_read.cpp - src/${PROJECT_NAME}/group_sync_write.cpp - src/${PROJECT_NAME}/group_bulk_read.cpp - src/${PROJECT_NAME}/group_bulk_write.cpp - src/${PROJECT_NAME}/port_handler.cpp - src/${PROJECT_NAME}_linux/port_handler_linux.cpp -) - -## Specify libraries to link a library or executable target against -target_link_libraries(dynamixel_sdk - ${catkin_LIBRARIES} -) - diff --git a/dynamixel_sdk/include/dynamixel_sdk.h b/dynamixel_sdk/include/dynamixel_sdk.h deleted file mode 100644 index a0fdf2a..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk.h +++ /dev/null @@ -1,59 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * dynamixel_sdk.h - * - * Created on: 2016. 3. 8. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ - - -#include "dynamixel_sdk/group_bulk_read.h" -#include "dynamixel_sdk/group_bulk_write.h" -#include "dynamixel_sdk/group_sync_read.h" -#include "dynamixel_sdk/group_sync_write.h" -#include "dynamixel_sdk/protocol1_packet_handler.h" -#include "dynamixel_sdk/protocol2_packet_handler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/port_handler_linux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/port_handler_windows.h" -#endif - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h deleted file mode 100644 index 29b0dba..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h +++ /dev/null @@ -1,92 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_read.h - * - * Created on: 2016. 1. 28. - * Author: zerom, leon - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupBulkRead -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - uint8_t *param_; - - void makeParam(); - - public: - GroupBulkRead(PortHandler *port, PacketHandler *ph); - ~GroupBulkRead() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length); - void removeParam (uint8_t id); - void clearParam (); - - int txPacket(); - int rxPacket(); - int txRxPacket(); - - bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); - uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h deleted file mode 100644 index 71bd9ec..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h +++ /dev/null @@ -1,87 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_write.h - * - * Created on: 2016. 2. 2. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupBulkWrite -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool is_param_changed_; - - uint8_t *param_; - uint16_t param_length_; - - void makeParam(); - - public: - GroupBulkWrite(PortHandler *port, PacketHandler *ph); - ~GroupBulkWrite() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); - void removeParam (uint8_t id); - bool changeParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); - void clearParam (); - - int txPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h deleted file mode 100644 index ec2a208..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h +++ /dev/null @@ -1,91 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_read.h - * - * Created on: 2016. 2. 2. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupSyncRead -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool last_result_; - bool is_param_changed_; - - uint8_t *param_; - uint16_t start_address_; - uint16_t data_length_; - - void makeParam(); - - public: - GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); - ~GroupSyncRead() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id); - void removeParam (uint8_t id); - void clearParam (); - - int txPacket(); - int rxPacket(); - int txRxPacket(); - - bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); - uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h b/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h deleted file mode 100644 index 1c85666..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h +++ /dev/null @@ -1,86 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_write.h - * - * Created on: 2016. 1. 28. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ - - -#include -#include -#include "port_handler.h" -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC GroupSyncWrite -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool is_param_changed_; - - uint8_t *param_; - uint16_t start_address_; - uint16_t data_length_; - - void makeParam(); - - public: - GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); - ~GroupSyncWrite() { clearParam(); } - - PortHandler *getPortHandler() { return port_; } - PacketHandler *getPacketHandler() { return ph_; } - - bool addParam (uint8_t id, uint8_t *data); - void removeParam (uint8_t id); - bool changeParam (uint8_t id, uint8_t *data); - void clearParam (); - - int txPacket(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h deleted file mode 100644 index 3816cbb..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h +++ /dev/null @@ -1,164 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * packet_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ - - -#include -#include -#include "port_handler.h" - -#define BROADCAST_ID 0xFE // 254 -#define MAX_ID 0xFC // 252 - -/* Macro for Control Table Value */ -#define DXL_MAKEWORD(a, b) ((unsigned short)(((unsigned char)(((unsigned long)(a)) & 0xff)) | ((unsigned short)((unsigned char)(((unsigned long)(b)) & 0xff))) << 8)) -#define DXL_MAKEDWORD(a, b) ((unsigned int)(((unsigned short)(((unsigned long)(a)) & 0xffff)) | ((unsigned int)((unsigned short)(((unsigned long)(b)) & 0xffff))) << 16)) -#define DXL_LOWORD(l) ((unsigned short)(((unsigned long)(l)) & 0xffff)) -#define DXL_HIWORD(l) ((unsigned short)((((unsigned long)(l)) >> 16) & 0xffff)) -#define DXL_LOBYTE(w) ((unsigned char)(((unsigned long)(w)) & 0xff)) -#define DXL_HIBYTE(w) ((unsigned char)((((unsigned long)(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_STATUS 85 // 0x55 -#define INST_SYNC_READ 130 // 0x82 -#define INST_BULK_WRITE 147 // 0x93 - -// 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 -{ - -class WINDECLSPEC PacketHandler -{ - protected: - PacketHandler() { } - - public: - static PacketHandler *getPacketHandler(float protocol_version = 2.0); - - virtual ~PacketHandler() { } - - virtual float getProtocolVersion() = 0; - - virtual void printTxRxResult(int result) = 0; - virtual void printRxPacketError(uint8_t error) = 0; - - virtual int txPacket (PortHandler *port, uint8_t *txpacket) = 0; - virtual int rxPacket (PortHandler *port, uint8_t *rxpacket) = 0; - virtual int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) = 0; - - virtual int ping (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; - virtual int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0; - - // broadcastPing - virtual int broadcastPing (PortHandler *port, std::vector &id_list) = 0; - - virtual int action (PortHandler *port, uint8_t id) = 0; - virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; - virtual int factoryReset (PortHandler *port, uint8_t id, uint8_t option = 0, uint8_t *error = 0) = 0; - - - virtual int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length) = 0; - virtual int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - virtual int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - virtual int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0) = 0; - virtual int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - virtual int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0) = 0; - virtual int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0) = 0; - - virtual int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - virtual int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0) = 0; - virtual int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0) = 0; - - virtual int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; - virtual int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) = 0; - virtual int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0; - - virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) = 0; - virtual int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0; - - virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) = 0; - virtual int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0; - - virtual int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; - virtual int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - 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 - - virtual int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; - - virtual int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - virtual int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h b/dynamixel_sdk/include/dynamixel_sdk/port_handler.h deleted file mode 100644 index dfc83fd..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/port_handler.h +++ /dev/null @@ -1,94 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ - -#undef __linux__ -#define __linux__ - -#ifdef __linux__ -#define WINDECLSPEC -#elif defined(_WIN32) || defined(_WIN64) -#ifdef WINDLLEXPORT -#define WINDECLSPEC __declspec(dllexport) -#else -#define WINDECLSPEC __declspec(dllimport) -#endif -#endif - -#include - -namespace dynamixel -{ - -class WINDECLSPEC PortHandler -{ - public: - static const int DEFAULT_BAUDRATE_ = 1000000; - - static PortHandler *getPortHandler(const char *port_name); - - bool is_using_; - - virtual ~PortHandler() { } - - virtual bool openPort() = 0; - virtual void closePort() = 0; - virtual void clearPort() = 0; - - virtual void setPortName(const char* port_name) = 0; - virtual char *getPortName() = 0; - - virtual bool setBaudRate(const int baudrate) = 0; - virtual int getBaudRate() = 0; - - virtual int getBytesAvailable() = 0; - - virtual int readPort(uint8_t *packet, int length) = 0; - virtual int writePort(uint8_t *packet, int length) = 0; - - virtual void setPacketTimeout(uint16_t packet_length) = 0; - virtual void setPacketTimeout(double msec) = 0; - virtual bool isPacketTimeout() = 0; -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h deleted file mode 100644 index 758556f..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h +++ /dev/null @@ -1,129 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol1_packet_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ - - -#include "packet_handler.h" - -namespace dynamixel -{ - -class WINDECLSPEC Protocol1PacketHandler : public PacketHandler -{ - private: - static Protocol1PacketHandler *unique_instance_; - - Protocol1PacketHandler(); - - public: - static Protocol1PacketHandler *getInstance() { return unique_instance_; } - - virtual ~Protocol1PacketHandler() { } - - float getProtocolVersion() { return 1.0; } - - void printTxRxResult(int result); - void printRxPacketError(uint8_t error); - - int txPacket (PortHandler *port, uint8_t *txpacket); - int rxPacket (PortHandler *port, uint8_t *rxpacket); - int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0); - - int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); - int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - - // broadcastPing - int broadcastPing (PortHandler *port, std::vector &id_list); - - int action (PortHandler *port, uint8_t id); - int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); - int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - - - int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); - int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); - int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - - int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); - int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - - int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); - int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - - int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); - int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - - int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); - int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - - int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); - int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - - int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - 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 - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - - // param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ... - int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h b/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h deleted file mode 100644 index ff7e2d1..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h +++ /dev/null @@ -1,134 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol2_packet_handler.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ - - -#include "packet_handler.h" - -namespace dynamixel -{ - -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: - static Protocol2PacketHandler *getInstance() { return unique_instance_; } - - virtual ~Protocol2PacketHandler() { } - - float getProtocolVersion() { return 2.0; } - - void printTxRxResult(int result); - void printRxPacketError(uint8_t error); - - int txPacket (PortHandler *port, uint8_t *txpacket); - int rxPacket (PortHandler *port, uint8_t *rxpacket); - int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0); - - int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); - int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - - // broadcastPing - int broadcastPing (PortHandler *port, std::vector &id_list); - - int action (PortHandler *port, uint8_t id); - int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); - int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - - - int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); - int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0); - int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0); - int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - - int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0); - int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - - int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); - int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0); - int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - - int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); - int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - - int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); - int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - - int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); - int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - - int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - 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 - - // param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn - int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - - // param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ... - int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - // param : 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 - int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h deleted file mode 100644 index 3a9ed24..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler_linux.h - * - * Created on: 2016. 1. 26. - */ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ - - -#include "dynamixel_sdk/port_handler.h" - -namespace dynamixel -{ - -class PortHandlerLinux : public PortHandler -{ - private: - int socket_fd_; - int baudrate_; - char port_name_[30]; - - 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: - PortHandlerLinux(const char *port_name); - virtual ~PortHandlerLinux() { closePort(); } - - bool openPort(); - void closePort(); - void clearPort(); - - void setPortName(const char *port_name); - char *getPortName(); - - bool setBaudRate(const int baudrate); - int getBaudRate(); - - int getBytesAvailable(); - - int readPort(uint8_t *packet, int length); - int writePort(uint8_t *packet, int length); - - void setPacketTimeout(uint16_t packet_length); - void setPacketTimeout(double msec); - bool isPacketTimeout(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h b/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h deleted file mode 100644 index 09583cb..0000000 --- a/dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: Leon Ryu Woon Jung */ - -/* -* port_handler_windows.h -* -* Created on: 2016. 4. 26. -*/ - -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ - -#include - -#include "dynamixel_sdk/port_handler.h" - -namespace dynamixel -{ -class WINDECLSPEC PortHandlerWindows : public PortHandler -{ - private: - HANDLE serial_handle_; - LARGE_INTEGER freq_, counter_; - - int baudrate_; - char port_name_[30]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte_; - - bool setupPort(const int baudrate); - - double getCurrentTime(); - double getTimeSinceStart(); - - public: - PortHandlerWindows(const char *port_name); - virtual ~PortHandlerWindows() { closePort(); } - - bool openPort(); - void closePort(); - void clearPort(); - - void setPortName(const char *port_name); - char *getPortName(); - - bool setBaudRate(const int baudrate); - int getBaudRate(); - - int getBytesAvailable(); - - int readPort(uint8_t *packet, int length); - int writePort(uint8_t *packet, int length); - - void setPacketTimeout(uint16_t packet_length); - void setPacketTimeout(double msec); - bool isPacketTimeout(); -}; - -} - - -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERWINDOWS_H_ */ diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml deleted file mode 100644 index ae8e683..0000000 --- a/dynamixel_sdk/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - dynamixel_sdk - 0.1.0 - The dynamixel_sdk package - - robotis - BSD - - ROBOTIS - - catkin - roscpp - roscpp - \ No newline at end of file diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp deleted file mode 100644 index 7ae113b..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp +++ /dev/null @@ -1,237 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_read.cpp - * - * Created on: 2016. 1. 28. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/group_bulk_read.h" - -using namespace dynamixel; - -GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0) -{ - 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]; - - 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]; - data_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]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_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) - 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_, length_list_[id], data_list_[id]); - if (result != COMM_SUCCESS) - { - fprintf(stderr, "[GroupBulkRead::rxPacket] ID %d result : %d !!!!!!!!!!\n", id, result); - 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; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp deleted file mode 100644 index 7b84151..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_bulk_write.cpp - * - * Created on: 2016. 2. 2. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/group_bulk_write.h" - -using namespace dynamixel; - -GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - 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) - makeParam(); - - return ph_->bulkWriteTxOnly(port_, param_, param_length_); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp deleted file mode 100644 index 13818cc..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp +++ /dev/null @@ -1,203 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_read.cpp - * - * Created on: 2016. 2. 2. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/group_sync_read.h" - -using namespace dynamixel; - -GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0), - 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_]; - - 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]; - data_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]]; - - id_list_.clear(); - data_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) - 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_, data_length_, data_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; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp b/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp deleted file mode 100644 index 94dea6f..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * group_sync_write.cpp - * - * Created on: 2016. 1. 28. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include "dynamixel_sdk/group_sync_write.h" - -using namespace dynamixel; - -GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - 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) - makeParam(); - - return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp deleted file mode 100644 index 04fdc20..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * packet_handler.cpp - * - * Created on: 2016. 1. 26. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/packet_handler.h" -#include "dynamixel_sdk/protocol1_packet_handler.h" -#include "dynamixel_sdk/protocol2_packet_handler.h" - -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()); -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp deleted file mode 100644 index 14c0c28..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler.cpp - * - * Created on: 2016. 2. 5. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk/port_handler.h" - -#ifdef __linux__ - #include "dynamixel_sdk_linux/port_handler_linux.h" -#endif - -#if defined(_WIN32) || defined(_WIN64) - #include "dynamixel_sdk_windows/port_handler_windows.h" -#endif - -using namespace dynamixel; - -PortHandler *PortHandler::getPortHandler(const char *port_name) -{ -#ifdef __linux__ - return (PortHandler *)(new PortHandlerLinux(port_name)); -#endif - -#if defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); -#endif -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp deleted file mode 100644 index 5129164..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ /dev/null @@ -1,727 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol1_packet_handler.cpp - * - * Created on: 2016. 1. 26. - */ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include "dynamixel_sdk/protocol1_packet_handler.h" - -#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() { } - -void Protocol1PacketHandler::printTxRxResult(int result) -{ - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; - - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; - - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; - - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; - - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; - - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; - - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; - - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; - - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; - - default: - break; - } -} - -void Protocol1PacketHandler::printRxPacketError(uint8_t error) -{ - if (error & ERRBIT_VOLTAGE) - printf("[RxPacketError] Input voltage error!\n"); - - if (error & ERRBIT_ANGLE) - printf("[RxPacketError] Angle limit error!\n"); - - if (error & ERRBIT_OVERHEAT) - printf("[RxPacketError] Overheat error!\n"); - - if (error & ERRBIT_RANGE) - printf("[RxPacketError] Out of range error!\n"); - - if (error & ERRBIT_CHECKSUM) - printf("[RxPacketError] Checksum error!\n"); - - if (error & ERRBIT_OVERLOAD) - printf("[RxPacketError] Overload error!\n"); - - if (error & ERRBIT_INSTRUCTION) - printf("[RxPacketError] Instruction code error!\n"); -} - -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 (int 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) -{ - 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] >= 0x64) // unavailable Error - { - // remove the first byte in the packet - for (uint8_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 (int 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 (uint8_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; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == action) == no need to wait for status packet - if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (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); - } - - // rx packet - result = rxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if (txpacket[PKT_ID] != rxpacket[PKT_ID]) - result = rxPacket(port, rxpacket); - - if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_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::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, 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]; - - result = rxPacket(port, rxpacket); - if (result == COMM_SUCCESS) - { - if (error != 0) - { - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - for (uint8_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 (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 = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS) - { - if (error != 0) - { - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - for (uint8_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 *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readRx(port, 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, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readRx(port, 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 COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} - -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]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_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}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_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) -{ - return COMM_NOT_AVAILABLE; -} -int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) -{ - return COMM_NOT_AVAILABLE; -} - -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]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_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}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; - - for (uint8_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]; - - 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 (uint8_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]; - - 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 (uint8_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 (int 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; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp deleted file mode 100644 index f814229..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ /dev/null @@ -1,1036 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * protocol2_packet_handler.cpp - * - * Created on: 2016. 1. 26. - */ - -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include -#include -#include -#include "dynamixel_sdk/protocol2_packet_handler.h" - -#define TXPACKET_MAX_LEN (4*1024) -#define RXPACKET_MAX_LEN (4*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() { } - -void Protocol2PacketHandler::printTxRxResult(int result) -{ - switch(result) - { - case COMM_SUCCESS: - printf("[TxRxResult] Communication success.\n"); - break; - - case COMM_PORT_BUSY: - printf("[TxRxResult] Port is in use!\n"); - break; - - case COMM_TX_FAIL: - printf("[TxRxResult] Failed transmit instruction packet!\n"); - break; - - case COMM_RX_FAIL: - printf("[TxRxResult] Failed get status packet from device!\n"); - break; - - case COMM_TX_ERROR: - printf("[TxRxResult] Incorrect instruction packet!\n"); - break; - - case COMM_RX_WAITING: - printf("[TxRxResult] Now recieving status packet!\n"); - break; - - case COMM_RX_TIMEOUT: - printf("[TxRxResult] There is no status packet!\n"); - break; - - case COMM_RX_CORRUPT: - printf("[TxRxResult] Incorrect status packet!\n"); - break; - - case COMM_NOT_AVAILABLE: - printf("[TxRxResult] Protocol does not support This function!\n"); - break; - - default: - break; - } -} - -void Protocol2PacketHandler::printRxPacketError(uint8_t error) -{ - if (error & ERRBIT_ALERT) - printf("[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!\n"); - - int not_alert_error = error & ~ERRBIT_ALERT; - - switch(not_alert_error) - { - case 0: - break; - - case ERRNUM_RESULT_FAIL: - printf("[RxPacketError] Failed to process the instruction packet!\n"); - break; - - case ERRNUM_INSTRUCTION: - printf("[RxPacketError] Undefined instruction or incorrect instruction!\n"); - break; - - case ERRNUM_CRC: - printf("[RxPacketError] CRC doesn't match!\n"); - break; - - case ERRNUM_DATA_RANGE: - printf("[RxPacketError] The data value is out of range!\n"); - break; - - case ERRNUM_DATA_LENGTH: - printf("[RxPacketError] The data length does not match as expected!\n"); - break; - - case ERRNUM_DATA_LIMIT: - printf("[RxPacketError] The data value exceeds the limit value!\n"); - break; - - case ERRNUM_ACCESS: - printf("[RxPacketError] Writing or Reading is not available to target address!\n"); - break; - - default: - printf("[RxPacketError] Unknown error code!\n"); - break; - } -} - -unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) -{ - uint16_t i; - uint16_t crc_table[256] = {0x0000, - 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 i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - uint8_t temp[TXPACKET_MAX_LEN] = {0}; - - for (uint8_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++) - temp[s] = packet[s]; // FF FF FD XX ID LEN_L LEN_H - //memcpy(temp, packet, PKT_LENGTH_H+1); - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC - { - temp[index++] = packet[i+PKT_INSTRUCTION]; - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD - temp[index++] = 0xFD; - packet_length_out++; - } - } - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; - - - ////////////////////////// - if (packet_length_in != packet_length_out) - packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t)); - - /////////////////////////// - - for (uint8_t s = 0; s < index; s++) - packet[s] = temp[s]; - //memcpy(packet, temp, index); - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); -} - -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) -{ - 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 || - 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 (uint8_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 (uint8_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; - - if (result == COMM_SUCCESS) - 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; - - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet - // (Instruction == action) == no need to wait for status packet - if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || - (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 - result = rxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if (txpacket[PKT_ID] != rxpacket[PKT_ID]) - result = rxPacket(port, rxpacket); - - if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_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}; - - 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)); - - 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 (uint8_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 (uint8_t s = 0; s < rx_length - 3; s++) - rxpacket[s] = rxpacket[3 + s]; - rx_length -= 3; - } - } - else - { - // remove unnecessary packets - for (uint8_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::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, 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 - //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing - - result = rxPacket(port, rxpacket); - if (result == COMM_SUCCESS) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - for (uint8_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 (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 = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - for (uint8_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 *data, uint8_t *error) -{ - uint8_t data_read[1] = {0}; - int result = readRx(port, 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, uint16_t *data, uint8_t *error) -{ - uint8_t data_read[2] = {0}; - int result = readRx(port, 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, uint32_t *data, uint8_t *error) -{ - uint8_t data_read[4] = {0}; - int result = readRx(port, 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); - //uint8_t *txpacket = new uint8_t[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_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); - //uint8_t *txpacket = new uint8_t[length+12]; - uint8_t rxpacket[11] = {0}; - - 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 (uint8_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); - //uint8_t *txpacket = new uint8_t[length+12]; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint8_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); - //uint8_t *txpacket = new uint8_t[length+12]; - uint8_t rxpacket[11] = {0}; - - 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 (uint8_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); - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for (uint8_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); - //uint8_t *txpacket = new uint8_t[param_length + 14]; - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for (uint8_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); - //uint8_t *txpacket = new uint8_t[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - - for (uint8_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 (int 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); - //uint8_t *txpacket = new uint8_t[param_length + 10]; - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; - - for (uint8_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; -} diff --git a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp deleted file mode 100644 index 0cc11e0..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp +++ /dev/null @@ -1,276 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: zerom, Leon Ryu Woon Jung */ - -/* - * port_handler_linux.cpp - * - * Created on: 2016. 1. 26. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "dynamixel_sdk_linux/port_handler_linux.h" - -#define LATENCY_TIMER 4 // msec (USB latency timer) - -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_, TCIOFLUSH); -} - -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) -{ - // 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; - } -} diff --git a/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp b/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp deleted file mode 100644 index c47e76f..0000000 --- a/dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/******************************************************************************* -* Copyright (c) 2016, ROBOTIS CO., LTD. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright notice, this -* list of conditions and the following disclaimer. -* -* * Redistributions in binary form must reproduce the above copyright notice, -* this list of conditions and the following disclaimer in the documentation -* and/or other materials provided with the distribution. -* -* * Neither the name of ROBOTIS nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Author: Leon Ryu Woon Jung */ - -/* -* PortHandlerWindows.cpp -* -* Created on: 2016. 4. 06. -*/ -#if defined(_WIN32) || defined(_WIN64) -#define WINDLLEXPORT -#endif - -#include "dynamixel_sdk_windows/port_handler_windows.h" - -#include -#include -#include - -#define LATENCY_TIMER 16 // msec (USB latency timer) - -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), "\\\\.\\", port_name); - setPortName(port_name); -} - -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; -}