From 587ba84c709e33b07990ef40ddc1fa0a58083397 Mon Sep 17 00:00:00 2001 From: ROBOTIS Date: Fri, 4 Mar 2016 21:01:35 +0900 Subject: [PATCH] - renewal --- README.md | 2 + dynamixel_sdk/CMakeLists.txt | 38 + .../include/dynamixel_sdk/GroupBulkRead.h | 59 + .../include/dynamixel_sdk/GroupBulkWrite.h | 55 + .../include/dynamixel_sdk/GroupSyncRead.h | 59 + .../include/dynamixel_sdk/GroupSyncWrite.h | 54 + .../include/dynamixel_sdk/PacketHandler.h | 130 ++ .../include/dynamixel_sdk/PortHandler.h | 51 + .../dynamixel_sdk/Protocol1PacketHandler.h | 95 ++ .../dynamixel_sdk/Protocol2PacketHandler.h | 100 ++ .../include/dynamixel_sdk/RobotisDef.h | 21 + .../dynamixel_sdk_linux/PortHandlerLinux.h | 62 + dynamixel_sdk/package.xml | 15 + .../src/dynamixel_sdk/GroupBulkRead.cpp | 202 +++ .../src/dynamixel_sdk/GroupBulkWrite.cpp | 133 ++ .../src/dynamixel_sdk/GroupSyncRead.cpp | 177 +++ .../src/dynamixel_sdk/GroupSyncWrite.cpp | 110 ++ .../src/dynamixel_sdk/PacketHandler.cpp | 22 + .../src/dynamixel_sdk/PortHandler.cpp | 31 + .../dynamixel_sdk/Protocol1PacketHandler.cpp | 562 +++++++++ .../dynamixel_sdk/Protocol2PacketHandler.cpp | 898 +++++++++++++ .../dynamixel_sdk_linux/PortHandlerLinux.cpp | 253 ++++ robotis_controller/CMakeLists.txt | 55 + .../robotis_controller/RobotisController.h | 124 ++ robotis_controller/package.xml | 38 + .../robotis_controller/RobotisController.cpp | 1112 +++++++++++++++++ robotis_controller_msgs/CMakeLists.txt | 47 + .../msg/JointCtrlModule.msg | 2 + robotis_controller_msgs/msg/SyncWriteItem.msg | 3 + robotis_controller_msgs/package.xml | 30 + .../srv/GetJointModule.srv | 4 + robotis_device/CMakeLists.txt | 61 + .../devices/dynamixel/GRIPPER.device | 70 ++ .../devices/dynamixel/H42-20-S300-R.device | 71 ++ .../devices/dynamixel/H54-100-S500-R.device | 71 ++ .../devices/dynamixel/H54-200-B500-R.device | 71 ++ .../devices/dynamixel/H54-200-S500-R.device | 71 ++ .../devices/dynamixel/L42-10-S300-R.device | 70 ++ .../devices/dynamixel/L54-30-S400-R.device | 71 ++ .../devices/dynamixel/L54-30-S500-R.device | 71 ++ .../devices/dynamixel/L54-50-S290-R.device | 71 ++ .../devices/dynamixel/L54-50-S500-R.device | 71 ++ .../devices/dynamixel/M42-10-S260-R.device | 71 ++ .../devices/dynamixel/M54-40-S250-R.device | 71 ++ .../devices/dynamixel/M54-60-S250-R.device | 71 ++ robotis_device/devices/dynamixel/MX-28.device | 59 + .../include/robotis_device/ControlTableItem.h | 52 + .../include/robotis_device/Dynamixel.h | 60 + .../include/robotis_device/DynamixelState.h | 76 ++ robotis_device/include/robotis_device/Robot.h | 35 + .../include/robotis_device/Sensor.h | 34 + robotis_device/package.xml | 34 + .../src/robotis_device/Dynamixel.cpp | 37 + robotis_device/src/robotis_device/Robot.cpp | 227 ++++ robotis_framework_common/CMakeLists.txt | 186 +++ .../robotis_framework_common/MotionModule.h | 53 + .../robotis_framework_common/RobotisDef.h | 21 + robotis_framework_common/package.xml | 52 + 58 files changed, 6452 insertions(+) create mode 100644 README.md create mode 100644 dynamixel_sdk/CMakeLists.txt create mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/PortHandler.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h create mode 100644 dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h create mode 100644 dynamixel_sdk/package.xml create mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp create mode 100644 dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp create mode 100644 robotis_controller/CMakeLists.txt create mode 100644 robotis_controller/include/robotis_controller/RobotisController.h create mode 100644 robotis_controller/package.xml create mode 100644 robotis_controller/src/robotis_controller/RobotisController.cpp create mode 100644 robotis_controller_msgs/CMakeLists.txt create mode 100644 robotis_controller_msgs/msg/JointCtrlModule.msg create mode 100644 robotis_controller_msgs/msg/SyncWriteItem.msg create mode 100644 robotis_controller_msgs/package.xml create mode 100644 robotis_controller_msgs/srv/GetJointModule.srv create mode 100644 robotis_device/CMakeLists.txt create mode 100644 robotis_device/devices/dynamixel/GRIPPER.device create mode 100644 robotis_device/devices/dynamixel/H42-20-S300-R.device create mode 100644 robotis_device/devices/dynamixel/H54-100-S500-R.device create mode 100644 robotis_device/devices/dynamixel/H54-200-B500-R.device create mode 100644 robotis_device/devices/dynamixel/H54-200-S500-R.device create mode 100644 robotis_device/devices/dynamixel/L42-10-S300-R.device create mode 100644 robotis_device/devices/dynamixel/L54-30-S400-R.device create mode 100644 robotis_device/devices/dynamixel/L54-30-S500-R.device create mode 100644 robotis_device/devices/dynamixel/L54-50-S290-R.device create mode 100644 robotis_device/devices/dynamixel/L54-50-S500-R.device create mode 100644 robotis_device/devices/dynamixel/M42-10-S260-R.device create mode 100644 robotis_device/devices/dynamixel/M54-40-S250-R.device create mode 100644 robotis_device/devices/dynamixel/M54-60-S250-R.device create mode 100644 robotis_device/devices/dynamixel/MX-28.device create mode 100644 robotis_device/include/robotis_device/ControlTableItem.h create mode 100644 robotis_device/include/robotis_device/Dynamixel.h create mode 100644 robotis_device/include/robotis_device/DynamixelState.h create mode 100644 robotis_device/include/robotis_device/Robot.h create mode 100644 robotis_device/include/robotis_device/Sensor.h create mode 100644 robotis_device/package.xml create mode 100644 robotis_device/src/robotis_device/Dynamixel.cpp create mode 100644 robotis_device/src/robotis_device/Robot.cpp create mode 100644 robotis_framework_common/CMakeLists.txt create mode 100644 robotis_framework_common/include/robotis_framework_common/MotionModule.h create mode 100644 robotis_framework_common/include/robotis_framework_common/RobotisDef.h create mode 100644 robotis_framework_common/package.xml diff --git a/README.md b/README.md new file mode 100644 index 0000000..73be6ba --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# ROBOTIS-Framework +ROBOTIS Framework GIT REP MAIN diff --git a/dynamixel_sdk/CMakeLists.txt b/dynamixel_sdk/CMakeLists.txt new file mode 100644 index 0000000..4f16e17 --- /dev/null +++ b/dynamixel_sdk/CMakeLists.txt @@ -0,0 +1,38 @@ +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}/PacketHandler.cpp + src/${PROJECT_NAME}/Protocol1PacketHandler.cpp + src/${PROJECT_NAME}/Protocol2PacketHandler.cpp + src/${PROJECT_NAME}/GroupSyncRead.cpp + src/${PROJECT_NAME}/GroupSyncWrite.cpp + src/${PROJECT_NAME}/GroupBulkRead.cpp + src/${PROJECT_NAME}/GroupBulkWrite.cpp + src/${PROJECT_NAME}/PortHandler.cpp + src/${PROJECT_NAME}_linux/PortHandlerLinux.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/GroupBulkRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h new file mode 100644 index 0000000..2447f87 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkRead.h @@ -0,0 +1,59 @@ +/* + * GroupBulkRead.h + * + * Created on: 2016. 1. 28. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ + + +#include +#include +#include "RobotisDef.h" +#include "PortHandler.h" +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class GroupBulkRead +{ +private: + PortHandler *port_; + PacketHandler *ph_; + + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // + + 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 GetData(UINT8_T id, UINT16_T address, UINT8_T *data); + bool GetData(UINT8_T id, UINT16_T address, UINT16_T *data); + bool GetData(UINT8_T id, UINT16_T address, UINT32_T *data); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h new file mode 100644 index 0000000..510af24 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupBulkWrite.h @@ -0,0 +1,55 @@ +/* + * GroupBulkWrite.h + * + * Created on: 2016. 2. 2. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ + + +#include +#include +#include "RobotisDef.h" +#include "PortHandler.h" +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class GroupBulkWrite +{ +private: + PortHandler *port_; + PacketHandler *ph_; + + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // + + 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 /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h new file mode 100644 index 0000000..900cd6f --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncRead.h @@ -0,0 +1,59 @@ +/* + * GroupSyncRead.h + * + * Created on: 2016. 2. 2. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ + + +#include +#include +#include "RobotisDef.h" +#include "PortHandler.h" +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class GroupSyncRead +{ +private: + PortHandler *port_; + PacketHandler *ph_; + + std::vector id_list_; + std::map data_list_; // + + 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 GetData(UINT8_T id, UINT16_T address, UINT8_T *data); + bool GetData(UINT8_T id, UINT16_T address, UINT16_T *data); + bool GetData(UINT8_T id, UINT16_T address, UINT32_T *data); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h new file mode 100644 index 0000000..720e60a --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/GroupSyncWrite.h @@ -0,0 +1,54 @@ +/* + * GroupSyncWrite.h + * + * Created on: 2016. 1. 28. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ + + +#include +#include +#include "RobotisDef.h" +#include "PortHandler.h" +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class GroupSyncWrite +{ +private: + PortHandler *port_; + PacketHandler *ph_; + + std::vector id_list_; + std::map data_list_; // + + 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 /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h new file mode 100644 index 0000000..3746831 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/PacketHandler.h @@ -0,0 +1,130 @@ +/* + * PacketHandler.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ + + +#include +#include "RobotisDef.h" +#include "PortHandler.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 ROBOTIS +{ + +class PacketHandler +{ +protected: + PacketHandler() { } + +public: + static PacketHandler *GetPacketHandler(float protocol_version = 2.0); + + virtual ~PacketHandler() { } + + virtual float GetProtocolVersion() = 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 /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h new file mode 100644 index 0000000..4406f50 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/PortHandler.h @@ -0,0 +1,51 @@ +/* + * PortHandler.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ + + +#include "RobotisDef.h" + +namespace ROBOTIS +{ + +class 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 /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h new file mode 100644 index 0000000..a1a9634 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol1PacketHandler.h @@ -0,0 +1,95 @@ +/* + * Protocol1PacketHandler.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ + + +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class Protocol1PacketHandler : public PacketHandler +{ +private: + static Protocol1PacketHandler *unique_instance_; + + Protocol1PacketHandler(); + +public: + static Protocol1PacketHandler *GetInstance() { return unique_instance_; } + + virtual ~Protocol1PacketHandler() { } + + float GetProtocolVersion() { return 1.0; } + + 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 /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h new file mode 100644 index 0000000..f37e9a9 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/Protocol2PacketHandler.h @@ -0,0 +1,100 @@ +/* + * Protocol2PacketHandler.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ + + +#include "PacketHandler.h" + +namespace ROBOTIS +{ + +class 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; } + + 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 /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h new file mode 100644 index 0000000..a860e9d --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk/RobotisDef.h @@ -0,0 +1,21 @@ +/* + * RobotisDef.h + * + * Created on: 2016. 1. 27. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ + + +typedef char INT8_T; +typedef short int INT16_T; +typedef int INT32_T; + +typedef unsigned char UINT8_T; +typedef unsigned short int UINT16_T; +typedef unsigned int UINT32_T; + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h new file mode 100644 index 0000000..2f3d487 --- /dev/null +++ b/dynamixel_sdk/include/dynamixel_sdk_linux/PortHandlerLinux.h @@ -0,0 +1,62 @@ +/* + * PortHandlerLinux.h + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ + + +#include "dynamixel_sdk/PortHandler.h" + +namespace ROBOTIS +{ + +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 /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ diff --git a/dynamixel_sdk/package.xml b/dynamixel_sdk/package.xml new file mode 100644 index 0000000..f21a3d3 --- /dev/null +++ b/dynamixel_sdk/package.xml @@ -0,0 +1,15 @@ + + + dynamixel_sdk + 0.1.0 + The dynamixel_sdk package + + robotis + GPLv2 + + ROBOTIS + + catkin + roscpp + roscpp + \ No newline at end of file diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp new file mode 100644 index 0000000..a97c4cc --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp @@ -0,0 +1,202 @@ +/* + * GroupBulkRead.cpp + * + * Created on: 2016. 1. 28. + * Author: zerom + */ + +#include +#include +#include "dynamixel_sdk/GroupBulkRead.h" + +using namespace ROBOTIS; + +GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) + : port_(port), + ph_(ph), + 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(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]; + + MakeParam(); + 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); + + MakeParam(); +} + +void GroupBulkRead::ClearParam() +{ + if(id_list_.size() != 0) + { + for(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(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; + + 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; + } + } + + return _result; +} + +int GroupBulkRead::TxRxPacket() +{ + int _result = COMM_TX_FAIL; + + _result = TxPacket(); + if(_result != COMM_SUCCESS) + return _result; + + return RxPacket(); +} + +bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) +{ + UINT16_T _start_addr, _data_length; + + if(data_list_.find(id) == data_list_.end()) + return false; + + _start_addr = address_list_[id]; + _data_length = length_list_[id]; + + if(address < _start_addr || _start_addr + _data_length - 1 < address) + return false; + + *data = data_list_[id][address - _start_addr]; + + return true; +} + +bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data) +{ + UINT16_T _start_addr, _data_length; + + if(data_list_.find(id) == data_list_.end()) + return false; + + _start_addr = address_list_[id]; + _data_length = length_list_[id]; + + if(address < _start_addr || _start_addr + _data_length - 2 < address) + return false; + + *data = DXL_MAKEWORD(data_list_[id][address - _start_addr], data_list_[id][address - _start_addr + 1]); + + return true; +} + +bool GroupBulkRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data) +{ + UINT16_T _start_addr, _data_length; + + if(data_list_.find(id) == data_list_.end()) + return false; + + _start_addr = address_list_[id]; + _data_length = length_list_[id]; + + if(address < _start_addr || _start_addr + _data_length - 4 < address) + return false; + + *data = 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])); + + return true; +} + diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp new file mode 100644 index 0000000..7096463 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp @@ -0,0 +1,133 @@ +/* + * GroupBulkWrite.cpp + * + * Created on: 2016. 2. 2. + * Author: zerom + */ + +#include +#include "dynamixel_sdk/GroupBulkWrite.h" + +using namespace ROBOTIS; + +GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) + : port_(port), + ph_(ph), + 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(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(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]; + + MakeParam(); + 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); + + MakeParam(); +} +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]; + + MakeParam(); + return true; +} +void GroupBulkWrite::ClearParam() +{ + if(ph_->GetProtocolVersion() == 1.0) + return; + + if(id_list_.size() != 0) + { + for(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; + + return ph_->BulkWriteTxOnly(port_, param_, param_length_); +} diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp new file mode 100644 index 0000000..5006a49 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp @@ -0,0 +1,177 @@ +/* + * GroupSyncRead.cpp + * + * Created on: 2016. 2. 2. + * Author: zerom + */ + +#include +#include "dynamixel_sdk/GroupSyncRead.h" + +using namespace ROBOTIS; + +GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) + : port_(port), + ph_(ph), + 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(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_]; + + MakeParam(); + 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); + + MakeParam(); +} +void GroupSyncRead::ClearParam() +{ + if(ph_->GetProtocolVersion() == 1.0) + return; + + if(id_list_.size() != 0) + { + for(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; + + return ph_->SyncReadTx(port_, start_address_, data_length_, param_, (UINT16_T)id_list_.size() * 1); +} + +int GroupSyncRead::RxPacket() +{ + 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; + } + + 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::GetData(UINT8_T id, UINT16_T address, UINT8_T *data) +{ + if(ph_->GetProtocolVersion() == 1.0) + return false; + + if(data_list_.find(id) == data_list_.end()) + return false; + + if(address < start_address_ || start_address_ + data_length_ - 1 < address) + return false; + + *data = data_list_[id][address - start_address_]; + + return true; +} +bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT16_T *data) +{ + if(ph_->GetProtocolVersion() == 1.0) + return false; + + if(data_list_.find(id) == data_list_.end()) + return false; + + if(address < start_address_ || start_address_ + data_length_ - 2 < address) + return false; + + *data = DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); + + return true; +} +bool GroupSyncRead::GetData(UINT8_T id, UINT16_T address, UINT32_T *data) +{ + if(ph_->GetProtocolVersion() == 1.0) + return false; + + if(data_list_.find(id) == data_list_.end()) + return false; + + if(address < start_address_ || start_address_ + data_length_ - 4 < address) + return false; + + *data = 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])); + + return true; +} + diff --git a/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp new file mode 100644 index 0000000..ac17423 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp @@ -0,0 +1,110 @@ +/* + * GroupSyncWrite.cpp + * + * Created on: 2016. 1. 28. + * Author: zerom + */ + +#include +#include "dynamixel_sdk/GroupSyncWrite.h" + +using namespace ROBOTIS; + +GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, UINT16_T start_address, UINT16_T data_length) + : port_(port), + ph_(ph), + 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(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]; + + MakeParam(); + 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); + + MakeParam(); +} + +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]; + + MakeParam(); + return true; +} + +void GroupSyncWrite::ClearParam() +{ + if(id_list_.size() != 0) + { + for(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; + + return ph_->SyncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); +} diff --git a/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp new file mode 100644 index 0000000..57579b0 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/PacketHandler.cpp @@ -0,0 +1,22 @@ +/* + * PacketHandler.cpp + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#include "dynamixel_sdk/PacketHandler.h" +#include "dynamixel_sdk/Protocol1PacketHandler.h" +#include "dynamixel_sdk/Protocol2PacketHandler.h" + +using namespace ROBOTIS; + +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/PortHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp new file mode 100644 index 0000000..2f1b5b0 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/PortHandler.cpp @@ -0,0 +1,31 @@ +/* + * PortHandler.cpp + * + * Created on: 2016. 2. 5. + * Author: zerom + */ + +#include "dynamixel_sdk/PortHandler.h" + +#ifdef __linux__ + #include "dynamixel_sdk_linux/PortHandlerLinux.h" +#endif + +#ifdef _WIN32 || _WIN64 + #include "dynamixel_sdk_windows/PortHandlerWindows.h" +#endif + +using namespace ROBOTIS; + +PortHandler *PortHandler::GetPortHandler(const char *port_name) +{ +#ifdef __linux__ + return (PortHandler *)(new PortHandlerLinux(port_name)); +#endif + +#ifdef _WIN32 || _WIN64 + return (PortHandler *)(new PortHandlerWindows(port_name)); +#endif +} + + diff --git a/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp new file mode 100644 index 0000000..a1a52b6 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol1PacketHandler.cpp @@ -0,0 +1,562 @@ +/* + * Protocol1PacketHandler.cpp + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#include +#include +#include "dynamixel_sdk/Protocol1PacketHandler.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 ROBOTIS; + +Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); + +Protocol1PacketHandler::Protocol1PacketHandler() { } + +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 + { + // re-calculate the exact length of the rx packet + _wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; + 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); + // TODO: check txpacket ID == rxpacket ID + + 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[2] = {0}; + _result = ReadTxRx(port, id, 0, 2, _data); // Address 0 : Model Number + if(_result == COMM_SUCCESS) + *model_number = DXL_MAKEWORD(_data[0], _data[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; + + _result = ReadTx(port, id, address, length); + if(_result != COMM_SUCCESS) + return _result; + + return ReadRx(port, length, data, error); +} + +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[1] = {0}; + int _result = ReadRx(port, 1, _data, error); + *data = _data[0]; + return _result; +} +int Protocol1PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read1ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read1ByteRx(port, data, error); +} + +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[2] = {0}; + int _result = ReadRx(port, 2, _data, error); + *data = DXL_MAKEWORD(_data[0], _data[1]); + return _result; +} +int Protocol1PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read2ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read2ByteRx(port, data, error); +} + +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+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_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[1] = { data }; + return WriteTxOnly(port, id, address, 1, _data); +} +int Protocol1PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) +{ + UINT8_T _data[1] = { data }; + return WriteTxRx(port, id, address, 1, _data, error); +} + +int Protocol1PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) +{ + UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return WriteTxOnly(port, id, address, 2, _data); +} +int Protocol1PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) +{ + UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return WriteTxRx(port, id, address, 2, _data, 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]; // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0+0] = start_address; + txpacket[PKT_PARAMETER0+1] = data_length; + + 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]; // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; + txpacket[PKT_PARAMETER0+0] = 0x00; + + 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/Protocol2PacketHandler.cpp b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp new file mode 100644 index 0000000..3cd3200 --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk/Protocol2PacketHandler.cpp @@ -0,0 +1,898 @@ +/* + * Protocol2PacketHandler.cpp + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#include +#include +#include +#include "dynamixel_sdk/Protocol2PacketHandler.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 ROBOTIS; + +Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); + +Protocol2PacketHandler::Protocol2PacketHandler() { } + +unsigned short Protocol2PacketHandler::UpdateCRC(UINT16_T crc_accum, UINT8_T *data_blk_ptr, UINT16_T data_blk_size) +{ + UINT16_T i, j; + UINT16_T crc_table[256] = {0x0000, + 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, + 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, + 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, + 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, + 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, + 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, + 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, + 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, + 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, + 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, + 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, + 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, + 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, + 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, + 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, + 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, + 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, + 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, + 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, + 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, + 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, + 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, + 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, + 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, + 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, + 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, + 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, + 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, + 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, + 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, + 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, + 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, + 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, + 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, + 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, + 0x820D, 0x8207, 0x0202 }; + + for(j = 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 - 2); _idx++) +// { +// if(rxpacket[_idx] == 0xFF && rxpacket[_idx+1] == 0xFF && rxpacket[_idx+2] == 0xFD) +// break; +// } + 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 + { + // re-calculate the exact length of the rx packet + _wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; + if(_wait_length > 100) + fprintf(stderr, "id : %d , _wait_length : %d \n", rxpacket[PKT_ID], _wait_length); + + if(_wait_length > RXPACKET_MAX_LEN - 10) + { + _result = COMM_RX_CORRUPT; + break; + } + + if(_rx_length < _wait_length) + { + // check timeout + if(port->IsPacketTimeout() == true) + { + if(_rx_length == 0) + _result = COMM_RX_TIMEOUT; + else { + _result = COMM_RX_CORRUPT; +// fprintf(stderr, "Fail Rx Packet(short_length1) : "); +// for(int _i = 0; _i < _rx_length + 11; _i++) +// { +// fprintf(stderr, " %x", rxpacket[_i]); +// } +// fprintf(stderr, "\n"); + } + 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; +// fprintf(stderr, "Rx Packet(crc_error) : "); +// for(int _i = 0; _i < _rx_length + 11; _i++) +// { +// fprintf(stderr, " %x", rxpacket[_i]); +// } +// fprintf(stderr, "\n"); + } + 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; +// fprintf(stderr, "Fail Rx Packet(short_length2): "); +// for(int _i = 0; _i < _rx_length + 11; _i++) +// { +// fprintf(stderr, " %x", rxpacket[_i]); +// } +// fprintf(stderr, "\n"); + } + 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); + // TODO: check txpacket ID == rxpacket ID + + 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) +{ + int _result = COMM_TX_FAIL; + + 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 = ( UINT8_T* )calloc(RXPACKET_MAX_LEN, sizeof(UINT8_T));//(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); + } +// else +// { +// fprintf(stderr, "Fail Rx Packet : "); +// for(int _i = 0; _i < length + 11; _i++) +// { +// fprintf(stderr, " %x", rxpacket[_i]); +// } +// fprintf(stderr, "\n"); +// } + + 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; + + _result = ReadTx(port, id, address, length); + if(_result != COMM_SUCCESS) + return _result; + + return ReadRx(port, length, data, error); +} + +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[1] = {0}; + int _result = ReadRx(port, 1, _data, error); + *data = _data[0]; + return _result; +} +int Protocol2PacketHandler::Read1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read1ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read1ByteRx(port, data, error); +} + +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[2] = {0}; + int _result = ReadRx(port, 2, _data, error); + *data = DXL_MAKEWORD(_data[0], _data[1]); + return _result; +} +int Protocol2PacketHandler::Read2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read2ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read2ByteRx(port, data, error); +} + +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[4] = {0}; + int _result = ReadRx(port, 4, _data, error); + *data = DXL_MAKEDWORD(DXL_MAKEWORD(_data[0], _data[1]), DXL_MAKEWORD(_data[2], _data[3])); + return _result; +} +int Protocol2PacketHandler::Read4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T *data, UINT8_T *error) +{ + int _result = COMM_TX_FAIL; + + _result = Read4ByteTx(port, id, address); + if(_result != COMM_SUCCESS) + return _result; + + return Read4ByteRx(port, data, error); +} + + +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[1] = { data }; + return WriteTxOnly(port, id, address, 1, _data); +} +int Protocol2PacketHandler::Write1ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT8_T data, UINT8_T *error) +{ + UINT8_T _data[1] = { data }; + return WriteTxRx(port, id, address, 1, _data, error); +} + +int Protocol2PacketHandler::Write2ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data) +{ + UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return WriteTxOnly(port, id, address, 2, _data); +} +int Protocol2PacketHandler::Write2ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT16_T data, UINT8_T *error) +{ + UINT8_T _data[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return WriteTxRx(port, id, address, 2, _data, error); +} + +int Protocol2PacketHandler::Write4ByteTxOnly(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data) +{ + UINT8_T _data[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); +} +int Protocol2PacketHandler::Write4ByteTxRx(PortHandler *port, UINT8_T id, UINT16_T address, UINT32_T data, UINT8_T *error) +{ + UINT8_T _data[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, 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/PortHandlerLinux.cpp b/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp new file mode 100644 index 0000000..6c6a40b --- /dev/null +++ b/dynamixel_sdk/src/dynamixel_sdk_linux/PortHandlerLinux.cpp @@ -0,0 +1,253 @@ +/* + * PortHandlerLinux.cpp + * + * Created on: 2016. 1. 26. + * Author: zerom + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dynamixel_sdk_linux/PortHandlerLinux.h" + +#define LATENCY_TIMER 4 // msec (USB latency timer) + +using namespace ROBOTIS; + +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); + } + + return true; +} + +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 timeval _tv; +// +// gettimeofday(&_tv, 0); +// +// return ((double)_tv.tv_sec * 1000.0 + (double)_tv.tv_usec / 1000.0); + + 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/robotis_controller/CMakeLists.txt b/robotis_controller/CMakeLists.txt new file mode 100644 index 0000000..a41b0d8 --- /dev/null +++ b/robotis_controller/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_controller) + +find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + sensor_msgs + std_msgs + robotis_device + robotis_controller_msgs + robotis_framework_common + cmake_modules + dynamixel_sdk +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES robotis_controller + CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +add_library(robotis_controller + src/robotis_controller/RobotisController.cpp +) + +## Specify libraries to link a library or executable target against +target_link_libraries(robotis_controller + yaml-cpp + robotis_device + dynamixel_sdk + ${catkin_LIBRARIES} +) diff --git a/robotis_controller/include/robotis_controller/RobotisController.h b/robotis_controller/include/robotis_controller/RobotisController.h new file mode 100644 index 0000000..f855a38 --- /dev/null +++ b/robotis_controller/include/robotis_controller/RobotisController.h @@ -0,0 +1,124 @@ +/* + * RobotisController.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ + + +#include +#include +#include +#include +#include + +#include "robotis_device/Robot.h" +#include "robotis_framework_common/MotionModule.h" + +#include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/GetJointModule.h" + +// TODO: TEMPORARY CODE !! +#include "dynamixel_sdk/GroupBulkRead.h" +#include "dynamixel_sdk/GroupSyncWrite.h" + +namespace ROBOTIS +{ + +enum CONTROLLER_MODE +{ + MOTION_MODULE_MODE, + DIRECT_CONTROL_MODE +}; + +class RobotisController +{ +private: + static RobotisController *unique_instance_; + + boost::thread queue_thread_; + boost::mutex queue_mutex_; + + bool is_timer_running_; + bool stop_timer_; + pthread_t timer_thread_; + CONTROLLER_MODE controller_mode_; + + std::list modules_; + std::vector direct_sync_write_; + + RobotisController(); + + void QueueThread(); + + bool CheckTimerStop(); + void InitSyncWrite(); + +public: + static const int CONTROL_CYCLE_MSEC = 8; // 8 msec + + bool DEBUG_PRINT; + Robot *robot; + + // TODO: TEMPORARY CODE !! + std::map port_to_bulk_read; + std::map port_to_sync_write; + + /* publisher */ + ros::Publisher goal_joint_state_pub; + ros::Publisher present_joint_state_pub; + ros::Publisher current_module_pub; + + static void *ThreadProc(void *param); + static RobotisController *GetInstance() { return unique_instance_; } + + bool Initialize(const std::string robot_file_path, const std::string init_file_path); + void Process(); + void AddModule(MotionModule *module); + void RemoveModule(MotionModule *module); + + void StartTimer(); + void StopTimer(); + bool IsTimerRunning(); + + void LoadOffset(const std::string path); + + /* ROS Topic Callback Functions */ + void SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg); + void SetControllerModeCallback(const std_msgs::String::ConstPtr &msg); + void SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); + void SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg); + bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res); + + int Ping (const std::string joint_name, UINT8_T *error = 0); + int Ping (const std::string joint_name, UINT16_T* model_number, UINT8_T *error = 0); + + int Action (const std::string joint_name); + int Reboot (const std::string joint_name, UINT8_T *error = 0); + int FactoryReset(const std::string joint_name, UINT8_T option = 0, UINT8_T *error = 0); + + int Read (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error = 0); + + int Read1Byte (const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error = 0); + int Read2Byte (const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error = 0); + int Read4Byte (const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error = 0); + + int Write (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); + int WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error = 0); + + int Write1Byte (const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error = 0); + int Write2Byte (const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error = 0); + int Write4Byte (const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error = 0); + + int RegWrite (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ */ diff --git a/robotis_controller/package.xml b/robotis_controller/package.xml new file mode 100644 index 0000000..c504764 --- /dev/null +++ b/robotis_controller/package.xml @@ -0,0 +1,38 @@ + + + robotis_controller + 0.1.1 + The robotis_controller package + + ROBOTIS + + + GPLv2 + + + + + + ROBOTIS + + + catkin + + roscpp + roslib + std_msgs + sensor_msgs + dynamixel_sdk + robotis_device + robotis_controller_msgs + robotis_framework_common + + roscpp + roslib + std_msgs + sensor_msgs + dynamixel_sdk + robotis_device + robotis_controller_msgs + + diff --git a/robotis_controller/src/robotis_controller/RobotisController.cpp b/robotis_controller/src/robotis_controller/RobotisController.cpp new file mode 100644 index 0000000..aa5bf7c --- /dev/null +++ b/robotis_controller/src/robotis_controller/RobotisController.cpp @@ -0,0 +1,1112 @@ +/* + * RobotisController.cpp + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#include +#include + +#include "robotis_controller/RobotisController.h" + +#define INDIRECT_ADDRESS "indirect_address_1" + +using namespace ROBOTIS; + +RobotisController *RobotisController::unique_instance_ = new RobotisController(); + +// INDIRECT ADDR : 634 -> Present Position (4 Byte) +// INDIRECT ADDR : 638 -> Present Velocity (4 Byte) +// INDIRECT ADDR : 642 -> Present Current (2 Byte) +// INDIRECT ADDR : 644 -> Input Voltage (2 Byte) +// INDIRECT ADDR : 646 -> Temperature (1 byte) +// INDIRECT ADDR : 647 -> External Port Data 1 (2 byte) +// INDIRECT ADDR : 649 -> External Port Data 2 (2 byte) +// INDIRECT ADDR : 651 -> External Port Data 3 (2 byte) +// INDIRECT ADDR : 653 -> External Port Data 4 (2 byte) +const UINT16_T PRESENT_POSITION_ADDR = 634; +const UINT16_T TORQUE_ENABLE_ADDR = 562; +const UINT16_T GOAL_POSITION_ADDR = 596; +const UINT16_T GOAL_VELOCITY_ADDR = 600; +const UINT16_T GOAL_ACCELERATION_ADDR = 606; +const UINT16_T EXT_PORT_DATA1_ADDR = 647; +const UINT16_T EXT_PORT_DATA2_ADDR = 649; +const UINT16_T EXT_PORT_DATA3_ADDR = 651; +const UINT16_T EXT_PORT_DATA4_ADDR = 653; + +RobotisController::RobotisController() +: is_timer_running_(false), + stop_timer_(false), + timer_thread_(0), + controller_mode_(MOTION_MODULE_MODE), + DEBUG_PRINT(false), + robot(0) +{ + direct_sync_write_.clear(); +} + +void RobotisController::InitSyncWrite() +{ + ROS_INFO("FIRST BULKREAD"); + // bulkread twice + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxRxPacket(); + usleep(50*1000); + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxRxPacket(); + ROS_INFO("FIRST BULKREAD END"); + + // clear syncwrite param setting + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + _it->second->ClearParam(); + + // set init syncwrite param(from data of bulkread) + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + { + INT32_T _pos = 0; + + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; + + bool _res = false; + _res = port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, (UINT32_T*)&_pos); + if(_res == true) + { + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset; + _dxl->dxl_state->goal_position = _dxl->dxl_state->present_position; + } + + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); + + port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + } +} + +bool RobotisController::Initialize(const std::string robot_file_path, const std::string init_file_path) +{ + std::string _dev_desc_dir_path = ros::package::getPath("robotis_device") + "/devices"; + + // load robot info : port , device + robot = new Robot(robot_file_path, _dev_desc_dir_path); + // TODO: fill joint_ctrl_module_ + + + // TODO: TEMPORARY CODE !! + /* temporary code start */ + PacketHandler *_protocol2_handler = PacketHandler::GetPacketHandler(2.0); + + for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) + { + port_to_bulk_read[_it->first] = new GroupBulkRead(_it->second, _protocol2_handler); + port_to_sync_write[_it->first] = new GroupSyncWrite(_it->second, _protocol2_handler, GOAL_POSITION_ADDR, 4); + } + + + // TODO: + // for loop ping --> no response : return false + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + { + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; + if(Ping(_joint_name) != 0) + { + usleep(10*1000); + if(Ping(_joint_name) != 0) + ROS_ERROR("JOINT[%s] does NOT response!!", _joint_name.c_str()); + } + } + + // joint(dynamixel) initialize + if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD"); + + YAML::Node _doc; + try{ + _doc = YAML::LoadFile(init_file_path.c_str()); + + for(YAML::const_iterator _it_doc = _doc.begin(); _it_doc != _doc.end(); _it_doc++) + { + std::string _joint_name = _it_doc->first.as(); + + YAML::Node _joint_node = _doc[_joint_name]; + if(_joint_node.size() == 0) + continue; + + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = robot->dxls.find(_joint_name); + if(_dxl_it != robot->dxls.end()) + _dxl = _dxl_it->second; + + if(_dxl == NULL) + { + ROS_WARN("Joint [%s] does not found.", _joint_name.c_str()); + continue; + } + if(DEBUG_PRINT) ROS_INFO("JOINT_NAME: %s", _joint_name.c_str()); + + for(YAML::const_iterator _it_joint = _joint_node.begin(); _it_joint != _joint_node.end(); _it_joint++) + { + std::string _item_name = _it_joint->first.as(); + + if(DEBUG_PRINT) ROS_INFO(" ITEM_NAME: %s", _item_name.c_str()); + // indirect address setting + if(_item_name == INDIRECT_ADDRESS) + { + YAML::Node _indirect_node = _joint_node[_item_name]; + if(_indirect_node.size() == 0) + continue; + + for(YAML::const_iterator _it_idx = _indirect_node.begin(); _it_idx != _indirect_node.end(); _it_idx++) + { + int _start_idx = _it_idx->first.as(); + if(_start_idx < 1 || 256 < _start_idx) + ROS_WARN("[%s] INDIRECT ADDRESS start index is out of range. (%d)", _joint_name.c_str(), _start_idx); + if(DEBUG_PRINT) ROS_INFO(" START_IDX: %d", _start_idx); + + YAML::Node _indirect_item_node = _indirect_node[_start_idx]; + if(_indirect_item_node.size() == 0) + continue; + + int _start_address = _dxl->ctrl_table[INDIRECT_ADDRESS]->address + _start_idx - 1; + for(unsigned int _i = 0; _i < _indirect_item_node.size(); _i++) + { + std::string _indir_item_name = _indirect_item_node[_i].as().c_str(); + int _addr_leng = _dxl->ctrl_table[_indir_item_name]->data_length; + for(int _l = 0; _l < _addr_leng; _l++) + { + if(DEBUG_PRINT) ROS_INFO(" - INDIR_ADDR[%d] : %d", _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); + Write2Byte(_joint_name, _start_address, _dxl->ctrl_table[_indir_item_name]->address + _l); + _start_address += 2; + } + } + } + } + // other items setting + else + { + UINT32_T _value = _it_joint->second.as(); + + ControlTableItem *_item = _dxl->ctrl_table[_item_name]; + if(_item == NULL) + { + ROS_WARN("Control Item [%s] does not found.", _item_name.c_str()); + continue; + } + + switch(_item->data_length) + { + case 1: + Write1Byte(_joint_name, _item->address, (UINT8_T)_value); + break; + case 2: + Write2Byte(_joint_name, _item->address, (UINT16_T)_value); + break; + case 4: + Write4Byte(_joint_name, _item->address, _value); + break; + default: + break; + } + } + } + } + } catch(const std::exception& e) { + ROS_INFO("Dynamixel Init file not found."); + } + + + // [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current ) + for(std::map::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++) + { + std::string _joint_name = _it->first; + Dynamixel *_dxl = _it->second; + + if(_dxl == NULL) + continue; + + UINT16_T _data_length = 0; + if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) + _data_length = 17; + else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) + _data_length = 21; + else + _data_length = 13; + port_to_bulk_read[_dxl->port_name]->AddParam(robot->dxls[_joint_name]->id, PRESENT_POSITION_ADDR, _data_length); + + // Torque ON + _protocol2_handler->Write1ByteTxRx(robot->ports[_dxl->port_name], robot->dxls[_joint_name]->id, TORQUE_ENABLE_ADDR, 1); + } + + //InitSyncWrite(); + + for(std::map::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++) + { + // set goal velocity = 0 + _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_VELOCITY_ADDR, 0); + // set goal acceleration = 0 + _protocol2_handler->Write4ByteTxOnly(_it->second, BROADCAST_ID, GOAL_ACCELERATION_ADDR, 0); + } + + /* temporary code end */ + + queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this)); + return true; +} + +void RobotisController::QueueThread() +{ + ros::NodeHandle _ros_node; + ros::CallbackQueue _callback_queue; + + _ros_node.setCallbackQueue(&_callback_queue); + + /* subscriber */ + ros::Subscriber _sync_write_item_sub= _ros_node.subscribe("/robotis/sync_write_item", 10, &RobotisController::SyncWriteItemCallback, this); + ros::Subscriber _ctrl_module_sub = _ros_node.subscribe("/robotis/set_ctrl_module", 10, &RobotisController::SetCtrlModuleCallback, this); + ros::Subscriber _controller_mode_sub= _ros_node.subscribe("/robotis/set_controller_mode", 10, &RobotisController::SetControllerModeCallback, this); + ros::Subscriber _direct_control_sub = _ros_node.subscribe("/robotis/set_joint_states", 10, &RobotisController::SetJointStatesCallback, this); + + /* publisher */ + goal_joint_state_pub = _ros_node.advertise("/robotis/goal_joint_states", 10); + present_joint_state_pub = _ros_node.advertise("/robotis/present_joint_states", 10); + current_module_pub = _ros_node.advertise("/robotis/current_ctrl_module", 10); + + /* service */ + ros::ServiceServer _joint_module_server = _ros_node.advertiseService("/robotis/get_ctrl_module", &RobotisController::GetCtrlModuleCallback, this); + + while(_ros_node.ok()) + { + _callback_queue.callAvailable(); + } +} + +void *RobotisController::ThreadProc(void *param) +{ + RobotisController *controller = (RobotisController *)param; + static struct timespec next_time; + static struct timespec curr_time; + + ROS_INFO("controller::thread_proc"); + + clock_gettime(CLOCK_MONOTONIC, &next_time); + + while(!controller->stop_timer_) + { + next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000; + next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000; + + controller->Process(); + + clock_gettime(CLOCK_MONOTONIC, &curr_time); + long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec); + if(delta_nsec < -0.3 ) + { + if(controller->DEBUG_PRINT == true) + ROS_WARN("[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]", delta_nsec/1000000.0, (long)next_time.tv_sec, (long)next_time.tv_nsec, (long)curr_time.tv_sec, (long)curr_time.tv_nsec); + // next_time = curr_time + 3 msec + next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000; + next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000; + } + + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); + } + return 0; +} + +void RobotisController::StartTimer() +{ + if(this->is_timer_running_ == true) + return; + + InitSyncWrite(); + + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxPacket(); + + int error; + struct sched_param param; + pthread_attr_t attr; + + pthread_attr_init(&attr); + + error = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if(error != 0) + ROS_ERROR("pthread_attr_setschedpolicy error = %d\n",error); + error = pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED); + if(error != 0) + ROS_ERROR("pthread_attr_setinheritsched error = %d\n",error); + + memset(¶m, 0, sizeof(param)); + param.sched_priority = 31;// RT + error = pthread_attr_setschedparam(&attr, ¶m); + if(error != 0) + ROS_ERROR("pthread_attr_setschedparam error = %d\n",error); + + // create and start the thread + if((error = pthread_create(&this->timer_thread_, &attr, this->ThreadProc, this))!= 0) { + ROS_ERROR("timer thread create fail!!"); + exit(-1); + } + + this->is_timer_running_ = true; +} + +void RobotisController::StopTimer() +{ + int error = 0; + + // set the flag to stop the thread + if(this->is_timer_running_) + { + this->stop_timer_ = true; + + // wait until the thread is stopped. + if((error = pthread_join(this->timer_thread_, NULL)) != 0) + exit(-1); + + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->RxPacket(); + + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + _it->second->ClearParam(); + + this->stop_timer_ = false; + this->is_timer_running_ = false; + } +} + +bool RobotisController::IsTimerRunning() +{ + return this->is_timer_running_; +} + +void RobotisController::LoadOffset(const std::string path) +{ + YAML::Node _doc; + try{ + _doc = YAML::LoadFile(path.c_str()); + } catch(const std::exception& e) { + ROS_ERROR("Fail to load offset yaml."); + return; + } + + YAML::Node _offset_node = _doc["offset"]; + if(_offset_node.size() == 0) + return; + + ROS_INFO("Load offsets..."); + for(YAML::const_iterator _it = _offset_node.begin(); _it != _offset_node.end(); _it++) + { + std::string _joint_name = _it->first.as(); + double _offset = _it->second.as(); + + std::map::iterator _dxl_it = robot->dxls.find(_joint_name); + if(_dxl_it != robot->dxls.end()) + _dxl_it->second->dxl_state->position_offset = _offset; + } +} + +void RobotisController::Process() +{ + static bool _is_process_running = false; + + if(_is_process_running == true) + return; + _is_process_running = true; + + // ROS_INFO("Controller::Process()"); + + sensor_msgs::JointState _present_state; + sensor_msgs::JointState _goal_state; + + ros::Time _now = ros::Time::now(); + + + // TODO: BulkRead Rx + bool _do_sync_write = false; + + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->RxPacket(); + + ros::Duration _dur = ros::Time::now() - _now; + double _msec = _dur.nsec * 0.000001; + + if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; + + // -> save to Robot->dxls[]->dynamixel_state.present_position + // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + { + UINT32_T _pos = 0; + + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, PRESENT_POSITION_ADDR, &_pos) == true) + _dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_pos) - _dxl->dxl_state->position_offset; // remove offset + + UINT16_T _ext_data = 0; + if(_dxl->id == 11 || _dxl->id == 12 || _dxl->id == 23 || _dxl->id == 24) + { + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[0] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[1] = _ext_data; + } + else if(_dxl->id == 13 || _dxl->id == 14 || _dxl->id == 25 || _dxl->id == 26) + { + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA1_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[0] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA2_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[1] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA3_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[2] = _ext_data; + if(port_to_bulk_read[_dxl->port_name]->GetData(_dxl->id, EXT_PORT_DATA4_ADDR, &_ext_data) == true) + _dxl->dxl_state->ext_port_data[3] = _ext_data; + } + + _present_state.name.push_back(_joint_name); + // TODO: should be converted to rad, rad/s, Nm + _present_state.position.push_back(_dxl->dxl_state->present_position); + _present_state.velocity.push_back(_dxl->dxl_state->present_velocity); + _present_state.effort.push_back(_dxl->dxl_state->present_load); + + _goal_state.name.push_back(_joint_name); + _goal_state.position.push_back(_dxl->dxl_state->goal_position); + _goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity); + _goal_state.effort.push_back(_dxl->dxl_state->goal_torque); + } + // -> publish present joint_states topic + _present_state.header.stamp = ros::Time::now(); + present_joint_state_pub.publish(_present_state); + + // -> publish goal joint_states topic + _goal_state.header.stamp = _present_state.header.stamp; + goal_joint_state_pub.publish(_goal_state); + + if(controller_mode_ == MOTION_MODULE_MODE) + { + // TODO: Call MotionModule Process() + // -> for loop : call MotionModule list -> Process() + if(modules_.size() > 0) + { + queue_mutex_.lock(); + + for(std::list::iterator module_it = modules_.begin(); module_it != modules_.end(); module_it++) + { + // ros::Time _before = ros::Time::now(); + + (*module_it)->Process(robot->dxls); + + // ros::Duration _dur = ros::Time::now() - _before; + // double _msec = _dur.nsec * 0.000001; + + // std::cout << "Process duration ["<< (*module_it)->module_name << "] : " << _msec << std::endl; + + // for loop : joint list + for(std::map::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++) + { + std::string _joint_name = dxl_it->first; + Dynamixel *_dxl = dxl_it->second; + + DynamixelState *_dxl_state = _dxl->dxl_state; + if(_dxl->ctrl_module_name == (*module_it)->module_name) + { + _do_sync_write = true; + // ROS_INFO("Set goal value"); + DynamixelState *_result_state = (*module_it)->result[_joint_name]; + + if(_result_state == NULL) { + ROS_INFO("[%s] %s", (*module_it)->module_name.c_str(), _joint_name.c_str()); + continue; + } + + // TODO: check update time stamp ? + + if((*module_it)->control_mode == POSITION_CONTROL) + { +// if(_result_state->goal_position == 0 && _dxl->id == 3) +// ROS_INFO("[MODULE:%s][ID:3] goal position = %f", (*module_it)->module_name.c_str(), _dxl_state->goal_position); + _dxl_state->goal_position = _result_state->goal_position; + + // add offset + UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl_state->goal_position + _dxl_state->position_offset); + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data)); + + if(abs(_pos_data) > 151800) + printf("goal_pos : %f | position_offset : %f | pos_data : %d\n", _dxl_state->goal_position , _dxl_state->position_offset, _pos_data); + + port_to_sync_write[_dxl->port_name]->ChangeParam(_dxl->id, _sync_write_data); + } + else if((*module_it)->control_mode == VELOCITY_CONTROL) + { + _dxl_state->goal_velocity = _result_state->goal_velocity; + } + else if((*module_it)->control_mode == TORQUE_CONTROL) + { + _dxl_state->goal_torque = _result_state->goal_torque; + } + } + } + } + + // std::cout << " ------------------------------------------- " << std::endl; + queue_mutex_.unlock(); + } + + // TODO: Combine the result && SyncWrite + // -> SyncWrite + if(_do_sync_write) { + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + _it->second->TxPacket(); + } + } + else if(controller_mode_ == DIRECT_CONTROL_MODE) + { + queue_mutex_.lock(); + + for(std::map::iterator _it = port_to_sync_write.begin(); _it != port_to_sync_write.end(); _it++) + { + _it->second->TxPacket(); + _it->second->ClearParam(); + } + + if(direct_sync_write_.size() > 0) + { + for(int _i = 0; _i < direct_sync_write_.size(); _i++) + { + direct_sync_write_[_i]->TxPacket(); + direct_sync_write_[_i]->ClearParam(); + } + direct_sync_write_.clear(); + } + + queue_mutex_.unlock(); + } + + // TODO: User Read/Write + + // TODO: BulkRead Tx + for(std::map::iterator _it = port_to_bulk_read.begin(); _it != port_to_bulk_read.end(); _it++) + _it->second->TxPacket(); + + // for test : goal to present + // for(std::map::iterator dxl_it = robot_->dxls.begin(); dxl_it != robot_->dxls.end(); dxl_it++) + // { + // dxl_it->second->dxl_state->present_position = dxl_it->second->dxl_state->goal_position; + // dxl_it->second->dxl_state->present_velocity = dxl_it->second->dxl_state->goal_velocity; + // } + + +// ros::Duration _dur = ros::Time::now() - _now; +// double _msec = _dur.nsec * 0.000001; +// +// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl; + + _is_process_running = false; +} + +void RobotisController::AddModule(MotionModule *module) +{ + module->Initialize(CONTROL_CYCLE_MSEC); + modules_.push_back(module); + modules_.unique(); +} + +void RobotisController::RemoveModule(MotionModule *module) +{ + modules_.remove(module); +} + +void RobotisController::SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg) +{ + for(int _i = 0; _i < msg->joint_name.size(); _i++) + { + Dynamixel *_dxl = robot->dxls[msg->joint_name[_i]]; + ControlTableItem *_item = _dxl->ctrl_table[msg->item_name]; + + PortHandler *_port = robot->ports[_dxl->port_name]; + PacketHandler *_packet_handler= PacketHandler::GetPacketHandler(_dxl->protocol_version); + + if(_item->access_type == READ) + continue; + + int _idx = 0; + if(direct_sync_write_.size() == 0) + { + direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); + _idx = 0; + } + else + { + for(_idx = 0; _idx < direct_sync_write_.size(); _idx++) + { + if(direct_sync_write_[_idx]->GetPortHandler() == _port && + direct_sync_write_[_idx]->GetPacketHandler() == _packet_handler) + break; + } + + if(_idx == direct_sync_write_.size()) + direct_sync_write_.push_back(new GroupSyncWrite(_port, _packet_handler, _item->address, _item->data_length)); + } + + UINT8_T *_data = new UINT8_T[_item->data_length]; + if(_item->data_length == 1) + _data[0] = (UINT8_T)msg->value[_i]; + else if(_item->data_length == 2) + { + _data[0] = DXL_LOBYTE((UINT16_T)msg->value[_i]); + _data[1] = DXL_HIBYTE((UINT16_T)msg->value[_i]); + } + else if(_item->data_length == 4) + { + _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); + _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)msg->value[_i])); + _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); + _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)msg->value[_i])); + } + direct_sync_write_[_idx]->AddParam(_dxl->id, _data); + delete[] _data; + } +} + +void RobotisController::SetControllerModeCallback(const std_msgs::String::ConstPtr &msg) +{ + if(msg->data == "DIRECT_CONTROL_MODE") + controller_mode_ = DIRECT_CONTROL_MODE; + else if(msg->data == "MOTION_MODULE_MODE") + controller_mode_ = MOTION_MODULE_MODE; +} + +void RobotisController::SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + if(controller_mode_ != DIRECT_CONTROL_MODE) + return; + + queue_mutex_.lock(); + + for(int _i = 0; _i < msg->name.size(); _i++) + { + INT32_T _pos = 0; + + Dynamixel *_dxl = robot->dxls[msg->name[_i]]; + if(_dxl == NULL) + continue; + + _dxl->dxl_state->goal_position = msg->position[_i]; + _pos = _dxl->ConvertRadian2Value((double)msg->position[_i]); + + UINT8_T _sync_write_data[4]; + _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos)); + _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos)); + _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos)); + _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos)); + + port_to_sync_write[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data); + } + + queue_mutex_.unlock(); +} + +void RobotisController::SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg) +{ + if(msg->joint_name.size() != msg->module_name.size()) + return; + + queue_mutex_.lock(); + + for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++) + { + Dynamixel *_dxl = NULL; + std::map::iterator _dxl_it = robot->dxls.find((std::string)(msg->joint_name[idx])); + if(_dxl_it != robot->dxls.end()) + _dxl = _dxl_it->second; + else + continue; + + // none + if(msg->module_name[idx] == "" || msg->module_name[idx] == "none") + { + _dxl->ctrl_module_name = msg->module_name[idx]; + continue; + } + + // check whether the module is using this joint + for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) + { + if((*_m_it)->module_name == msg->module_name[idx]) + { + if((*_m_it)->result.find(msg->joint_name[idx]) != (*_m_it)->result.end()) + { + _dxl->ctrl_module_name = msg->module_name[idx]; + break; + } + } + } + } + + for(std::list::iterator _m_it = modules_.begin(); _m_it != modules_.end(); _m_it++) + { + // set all modules -> disable + (*_m_it)->enable = false; + + // set all used modules -> enable + for(std::map::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++) + { + if(_d_it->second->ctrl_module_name == (*_m_it)->module_name) + { + (*_m_it)->enable = true; + break; + } + } + } + + // TODO: set indirect address + // -> check module's control_mode + + queue_mutex_.unlock(); + + robotis_controller_msgs::JointCtrlModule _current_module_msg; + for(std::map::iterator _dxl_iter = robot->dxls.begin(); _dxl_iter != robot->dxls.end(); ++_dxl_iter) + { + _current_module_msg.joint_name.push_back(_dxl_iter->first); + _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name); + } + + if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size()) + current_module_pub.publish(_current_module_msg); +} + +bool RobotisController::GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res) +{ + for(unsigned int idx = 0; idx < req.joint_name.size(); idx++) + { + std::map::iterator _d_it = robot->dxls.find((std::string)(req.joint_name[idx])); + if(_d_it != robot->dxls.end()) + { + res.joint_name.push_back(req.joint_name[idx]); + res.module_name.push_back(_d_it->second->ctrl_module_name); + } + } + + if(res.joint_name.size() == 0) return false; + + return true; +} + +bool RobotisController::CheckTimerStop() +{ + if(this->is_timer_running_) + { + if(DEBUG_PRINT == true) + ROS_WARN("Process Timer is running.. STOP the timer first."); + return false; + } + return true; +} + +int RobotisController::Ping(const std::string joint_name, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Ping(_port_handler, _dxl->id, error); +} +int RobotisController::Ping(const std::string joint_name, UINT16_T* model_number, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Ping(_port_handler, _dxl->id, model_number, error); +} + +int RobotisController::Action(const std::string joint_name) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Action(_port_handler, _dxl->id); +} +int RobotisController::Reboot(const std::string joint_name, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Reboot(_port_handler, _dxl->id, error); +} +int RobotisController::FactoryReset(const std::string joint_name, UINT8_T option, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->FactoryReset(_port_handler, _dxl->id, option, error); +} + +int RobotisController::Read(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->ReadTxRx(_port_handler, _dxl->id, address, length, data, error); +} + +int RobotisController::ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem*_item = _dxl->ctrl_table[item_name]; + if(_item == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + int _result = COMM_NOT_AVAILABLE; + switch(_item->data_length) + { + case 1: + { + UINT8_T _data = 0; + _result = _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); + if(_result == COMM_SUCCESS) + *data = _data; + break; + } + case 2: + { + UINT16_T _data = 0; + _result = _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); + if(_result == COMM_SUCCESS) + *data = _data; + break; + } + case 4: + { + UINT32_T _data = 0; + _result = _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, _item->address, &_data, error); + if(_result == COMM_SUCCESS) + *data = _data; + break; + } + default: + break; + } + return _result; +} + +int RobotisController::Read1Byte(const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Read1ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Read2Byte(const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Read2ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Read4Byte(const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Read4ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Write(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->WriteTxRx(_port_handler, _dxl->id, address, length, data, error); +} + +int RobotisController::WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + ControlTableItem*_item = _dxl->ctrl_table[item_name]; + if(_item == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + int _result = COMM_NOT_AVAILABLE; + UINT8_T *_data = new UINT8_T[_item->data_length]; + if(_item->data_length == 1) + { + _data[0] = (UINT8_T)data; + _result = _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); + } + else if(_item->data_length == 2) + { + _data[0] = DXL_LOBYTE((UINT16_T)data); + _data[1] = DXL_HIBYTE((UINT16_T)data); + _result = _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); + } + else if(_item->data_length == 4) + { + _data[0] = DXL_LOBYTE(DXL_LOWORD((UINT32_T)data)); + _data[1] = DXL_HIBYTE(DXL_LOWORD((UINT32_T)data)); + _data[2] = DXL_LOBYTE(DXL_HIWORD((UINT32_T)data)); + _data[3] = DXL_HIBYTE(DXL_HIWORD((UINT32_T)data)); + _result = _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, _item->address, data, error); + } + delete[] _data; + return _result; +} + +int RobotisController::Write1Byte(const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Write1ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Write2Byte(const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Write2ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::Write4Byte(const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->Write4ByteTxRx(_port_handler, _dxl->id, address, data, error); +} + +int RobotisController::RegWrite(const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error) +{ + if(CheckTimerStop() == false) + return COMM_PORT_BUSY; + + Dynamixel *_dxl = robot->dxls[joint_name]; + if(_dxl == NULL) + return COMM_NOT_AVAILABLE; + + PacketHandler *_pkt_handler = PacketHandler::GetPacketHandler(_dxl->protocol_version); + PortHandler *_port_handler = robot->ports[_dxl->port_name]; + + return _pkt_handler->RegWriteTxRx(_port_handler, _dxl->id, address, length, data, error); +} + diff --git a/robotis_controller_msgs/CMakeLists.txt b/robotis_controller_msgs/CMakeLists.txt new file mode 100644 index 0000000..221d97d --- /dev/null +++ b/robotis_controller_msgs/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_controller_msgs) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + sensor_msgs + std_msgs + message_generation +) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + SyncWriteItem.msg + JointCtrlModule.msg +) + +## Generate services in the 'srv' folder +add_service_files( + FILES + GetJointModule.srv +) + # Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + sensor_msgs +) + +catkin_package( +# INCLUDE_DIRS include + CATKIN_DEPENDS sensor_msgs std_msgs +) + diff --git a/robotis_controller_msgs/msg/JointCtrlModule.msg b/robotis_controller_msgs/msg/JointCtrlModule.msg new file mode 100644 index 0000000..b91eb4d --- /dev/null +++ b/robotis_controller_msgs/msg/JointCtrlModule.msg @@ -0,0 +1,2 @@ +string[] joint_name +string[] module_name \ No newline at end of file diff --git a/robotis_controller_msgs/msg/SyncWriteItem.msg b/robotis_controller_msgs/msg/SyncWriteItem.msg new file mode 100644 index 0000000..4d602b6 --- /dev/null +++ b/robotis_controller_msgs/msg/SyncWriteItem.msg @@ -0,0 +1,3 @@ +string item_name +string[] joint_name +uint32[] value \ No newline at end of file diff --git a/robotis_controller_msgs/package.xml b/robotis_controller_msgs/package.xml new file mode 100644 index 0000000..88a4148 --- /dev/null +++ b/robotis_controller_msgs/package.xml @@ -0,0 +1,30 @@ + + + robotis_controller_msgs + 0.1.0 + The robotis_controller_msgs package + robotis + + GPLv2 + + + + robotis --> + + catkin + + sensor_msgs + std_msgs + message_generation + + sensor_msgs + std_msgs + message_runtime + + + + + + + + \ No newline at end of file diff --git a/robotis_controller_msgs/srv/GetJointModule.srv b/robotis_controller_msgs/srv/GetJointModule.srv new file mode 100644 index 0000000..bedde91 --- /dev/null +++ b/robotis_controller_msgs/srv/GetJointModule.srv @@ -0,0 +1,4 @@ +string[] joint_name +--- +string[] joint_name +string[] module_name \ No newline at end of file diff --git a/robotis_device/CMakeLists.txt b/robotis_device/CMakeLists.txt new file mode 100644 index 0000000..6580f81 --- /dev/null +++ b/robotis_device/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_device) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + dynamixel_sdk + robotis_framework_common +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES robotis_device + CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(robotis_device + src/robotis_device/Robot.cpp + src/robotis_device/Dynamixel.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(robotis_device ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(robotis_device_node src/robotis_device_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(robotis_device_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(robotis_device + dynamixel_sdk + ${catkin_LIBRARIES} +) diff --git a/robotis_device/devices/dynamixel/GRIPPER.device b/robotis_device/devices/dynamixel/GRIPPER.device new file mode 100644 index 0000000..42de920 --- /dev/null +++ b/robotis_device/devices/dynamixel/GRIPPER.device @@ -0,0 +1,70 @@ +[device info] +model_name = GRIPPER +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = 0 +max_radian_position_value = 750 +min_radian = 0 +max_radian = 1.150767 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H42-20-S300-R.device b/robotis_device/devices/dynamixel/H42-20-S300-R.device new file mode 100644 index 0000000..f5ae090 --- /dev/null +++ b/robotis_device/devices/dynamixel/H42-20-S300-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = H42-20-S300-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -151900 +max_radian_position_value = 151900 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-100-S500-R.device b/robotis_device/devices/dynamixel/H54-100-S500-R.device new file mode 100644 index 0000000..4043cad --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-100-S500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = H54-100-S500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -250950 +max_radian_position_value = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-200-B500-R.device b/robotis_device/devices/dynamixel/H54-200-B500-R.device new file mode 100644 index 0000000..c3f82c5 --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-200-B500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = H54-200-B500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -250950 +max_radian_position_value = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/H54-200-S500-R.device b/robotis_device/devices/dynamixel/H54-200-S500-R.device new file mode 100644 index 0000000..e29ba3e --- /dev/null +++ b/robotis_device/devices/dynamixel/H54-200-S500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = H54-200-S500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -250950 +max_radian_position_value = 250950 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L42-10-S300-R.device b/robotis_device/devices/dynamixel/L42-10-S300-R.device new file mode 100644 index 0000000..057d9a6 --- /dev/null +++ b/robotis_device/devices/dynamixel/L42-10-S300-R.device @@ -0,0 +1,70 @@ +[device info] +model_name = L42-10-S300-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -2047 +max_radian_position_value = 2048 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-30-S400-R.device b/robotis_device/devices/dynamixel/L54-30-S400-R.device new file mode 100644 index 0000000..13e8d55 --- /dev/null +++ b/robotis_device/devices/dynamixel/L54-30-S400-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = L54-30-S400-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -144198 +max_radian_position_value = 144198 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-30-S500-R.device b/robotis_device/devices/dynamixel/L54-30-S500-R.device new file mode 100644 index 0000000..000fdb9 --- /dev/null +++ b/robotis_device/devices/dynamixel/L54-30-S500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = L54-30-S500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -180684 +max_radian_position_value = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-50-S290-R.device b/robotis_device/devices/dynamixel/L54-50-S290-R.device new file mode 100644 index 0000000..1bf2115 --- /dev/null +++ b/robotis_device/devices/dynamixel/L54-50-S290-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = L54-50-S290-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -103860 +max_radian_position_value = 103860 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/L54-50-S500-R.device b/robotis_device/devices/dynamixel/L54-50-S500-R.device new file mode 100644 index 0000000..8fcd40c --- /dev/null +++ b/robotis_device/devices/dynamixel/L54-50-S500-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = L54-50-S500-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -180684 +max_radian_position_value = 180684 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/M42-10-S260-R.device b/robotis_device/devices/dynamixel/M42-10-S260-R.device new file mode 100644 index 0000000..d9836d8 --- /dev/null +++ b/robotis_device/devices/dynamixel/M42-10-S260-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = M42-10-S260-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -131584 +max_radian_position_value = 131584 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/M54-40-S250-R.device b/robotis_device/devices/dynamixel/M54-40-S250-R.device new file mode 100644 index 0000000..ba3d1ac --- /dev/null +++ b/robotis_device/devices/dynamixel/M54-40-S250-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = M54-40-S250-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -125700 +max_radian_position_value = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/M54-60-S250-R.device b/robotis_device/devices/dynamixel/M54-60-S250-R.device new file mode 100644 index 0000000..ae9cbed --- /dev/null +++ b/robotis_device/devices/dynamixel/M54-60-S250-R.device @@ -0,0 +1,71 @@ +[device info] +model_name = M54-60-S250-R +device_type = Dynamixel + +[type info] +0_radian_position_value = 0 +min_radian_position_value = -125700 +max_radian_position_value = 125700 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 562 +position_d_gain_address = +position_i_gain_address = +position_p_gain_address = 594 +goal_position_address = 596 +goal_velocity_address = 600 +goal_torque_address = 604 +present_position_address = 611 +present_velocity_address = 615 +present_load_address = 621 +is_moving_address = 610 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 7 | ID | 1 | RW | EEPROM | 0 | 252 | N + 8 | baudrate | 1 | RW | EEPROM | 0 | 8 | N + 9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 11 | operating_mode | 1 | RW | EEPROM | 0 | 4 | N + 13 | homing_offset | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 17 | moving_threshold | 4 | RW | EEPROM | 0 | 2147483647 | N + 21 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N + 22 | max_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 24 | min_voltage_limit | 2 | RW | EEPROM | 0 | 400 | N + 26 | acceleration_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 30 | torque_limit | 2 | RW | EEPROM | 0 | 32767 | N + 32 | velocity_limit | 4 | RW | EEPROM | 0 | 2147483647 | N + 36 | max_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 40 | min_position_limit | 4 | RW | EEPROM | -2147483648 | 2147483647 | Y + 44 | external_port_mod_1 | 1 | RW | EEPROM | 0 | 3 | N + 45 | external_port_mod_2 | 1 | RW | EEPROM | 0 | 3 | N + 46 | external_port_mod_3 | 1 | RW | EEPROM | 0 | 3 | N + 47 | external_port_mod_4 | 1 | RW | EEPROM | 0 | 3 | N + 48 | shutdown | 1 | RW | EEPROM | 0 | 255 | N + 49 | indirect_address_1 | 2 | RW | EEPROM | 0 | 65535 | N + 562 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 563 | LED_RED | 1 | RW | RAM | 0 | 255 | N + 564 | LED_GREEN | 1 | RW | RAM | 0 | 255 | N + 565 | LED_BLUE | 1 | RW | RAM | 0 | 255 | N + 586 | velocity_I_gain | 2 | RW | RAM | 0 | 2047 | N + 588 | velocity_P_gain | 2 | RW | RAM | 0 | 2047 | N + 594 | position_P_gain | 2 | RW | RAM | 0 | 2047 | N + 596 | goal_position | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 600 | goal_velocity | 4 | RW | RAM | -2147483648 | 2147483647 | Y + 604 | goal_torque | 2 | RW | RAM | -32768 | 32767 | Y + 606 | goal_acceleration | 4 | RW | RAM | 0 | 2147483647 | N + 610 | is_moving | 1 | R | RAM | 0 | 1 | N + 611 | present_position | 4 | R | RAM | -2147483648 | 2147483647 | Y + 615 | present_velocity | 4 | R | RAM | -2147483648 | 2147483647 | Y + 621 | present_current | 2 | R | RAM | -32768 | 32767 | Y + 623 | present_voltage | 2 | R | RAM | 0 | 500 | N + 625 | present_temperature | 1 | R | RAM | 0 | 200 | N + 626 | external_port_data_1 | 2 | RW | RAM | 0 | 4095 | N + 628 | external_port_data_2 | 2 | RW | RAM | 0 | 4095 | N + 630 | external_port_data_3 | 2 | RW | RAM | 0 | 4095 | N + 632 | external_port_data_4 | 2 | RW | RAM | 0 | 4095 | N + 634 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N + 890 | registerd_instruction | 1 | R | RAM | 0 | 1 | N + 891 | status_return_level | 1 | RW | RAM | 0 | 2 | N + 892 | hardware_error_status | 2 | R | RAM | 0 | 31 | N diff --git a/robotis_device/devices/dynamixel/MX-28.device b/robotis_device/devices/dynamixel/MX-28.device new file mode 100644 index 0000000..ef59e83 --- /dev/null +++ b/robotis_device/devices/dynamixel/MX-28.device @@ -0,0 +1,59 @@ +[device info] +model_name = MX-28 +device_type = Dynamixel + +[type info] +0_radian_position_value = 2048 +min_radian_position_value = 0 +max_radian_position_value = 4095 +min_radian = -3.14159265 +max_radian = 3.14159265 +torque_enable_address = 24 +position_d_gain_address = 26 +position_i_gain_address = 27 +position_p_gain_address = 28 +goal_position_address = 30 +goal_velocity_address = 32 +goal_torque_address = 34 +present_position_address = 36 +present_velocity_address = 38 +present_load_address = 40 +is_moving_address = 46 + +[control table] +# addr | item name | length | access | memory | min value | max value | signed + 0 | model_number | 2 | R | EEPROM | 0 | 65535 | N + 2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N + 3 | ID | 1 | RW | EEPROM | 0 | 252 | N + 4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N + 5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N + 6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N + 10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N + 11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N + 12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N + 14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N + 16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N + 17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N + 18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N + 20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y + 22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N + 24 | torque_enable | 1 | RW | RAM | 0 | 1 | N + 25 | LED | 1 | RW | RAM | 0 | 1 | N + 26 | position_D_gain | 1 | RW | RAM | 0 | 254 | N + 27 | position_I_gain | 1 | RW | RAM | 0 | 254 | N + 28 | position_P_gain | 1 | RW | RAM | 0 | 254 | N + 30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y + 32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N + 34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N + 36 | present_position | 2 | R | RAM | 0 | 4095 | N + 38 | present_velocity | 2 | R | RAM | 0 | 2048 | N + 40 | present_load | 2 | R | RAM | 0 | 2048 | N + 42 | present_voltage | 1 | R | RAM | 50 | 250 | N + 43 | present_temperature | 1 | R | RAM | 0 | 99 | N + 44 | registered_instruction | 1 | R | RAM | 0 | 1 | N + 46 | is_moving | 1 | R | RAM | 0 | 1 | N + 47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N + 48 | punch | 2 | RW | RAM | 0 | 1023 | N + 73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N diff --git a/robotis_device/include/robotis_device/ControlTableItem.h b/robotis_device/include/robotis_device/ControlTableItem.h new file mode 100644 index 0000000..5a48bab --- /dev/null +++ b/robotis_device/include/robotis_device/ControlTableItem.h @@ -0,0 +1,52 @@ +/* + * ControlTableItem.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ + + +#include + +namespace ROBOTIS +{ + +enum ACCESS_TYPE { + READ, + READ_WRITE +}; + +enum MEMORY_TYPE { + EEPROM, + RAM +}; + +class ControlTableItem +{ +public: + UINT16_T address; + ACCESS_TYPE access_type; + MEMORY_TYPE memory_type; + UINT8_T data_length; + INT32_T data_min_value; + INT32_T data_max_value; + bool is_signed; + + ControlTableItem() + : address(0), + access_type(READ), + memory_type(RAM), + data_length(0), + data_min_value(0), + data_max_value(0), + is_signed(false) + { } +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_CONTROLTABLEITEM_H_ */ diff --git a/robotis_device/include/robotis_device/Dynamixel.h b/robotis_device/include/robotis_device/Dynamixel.h new file mode 100644 index 0000000..4649044 --- /dev/null +++ b/robotis_device/include/robotis_device/Dynamixel.h @@ -0,0 +1,60 @@ +/* + * Dynamixel.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ + + +#include +#include +#include "DynamixelState.h" +#include "ControlTableItem.h" + +namespace ROBOTIS +{ + +class Dynamixel +{ +public: + UINT8_T id; + std::string model_name; + float protocol_version; + + std::map ctrl_table; // string: item name + + std::string port_name; + std::string ctrl_module_name; + DynamixelState *dxl_state; + + INT32_T zero_radian_position_value; + INT32_T min_radian_position_value; + INT32_T max_radian_position_value; + double min_radian; + double max_radian; + + UINT16_T torque_enable_address; + UINT16_T position_d_gain_address; + UINT16_T position_i_gain_address; + UINT16_T position_p_gain_address; + UINT16_T goal_position_address; + UINT16_T goal_velocity_address; + UINT16_T goal_torque_address; + UINT16_T present_position_address; + UINT16_T present_velocity_address; + UINT16_T present_load_address; + UINT16_T is_moving_address; + + Dynamixel(int id, std::string model_name, float protocol_version); + + double ConvertValue2Radian(int32_t value) { return value * max_radian / max_radian_position_value; } + INT32_T ConvertRadian2Value(double radian) { return radian * max_radian_position_value / max_radian; } +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ */ diff --git a/robotis_device/include/robotis_device/DynamixelState.h b/robotis_device/include/robotis_device/DynamixelState.h new file mode 100644 index 0000000..7917aed --- /dev/null +++ b/robotis_device/include/robotis_device/DynamixelState.h @@ -0,0 +1,76 @@ +/* + * DynamixelState.h + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ + +#include +#include + +namespace ROBOTIS +{ + +class TimeStamp { +public: + long sec; + long nsec; + TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { } +}; + +class DynamixelState +{ +public: + TimeStamp update_time_stamp; + + bool torque_enable; + UINT16_T position_d_gain; + UINT16_T position_i_gain; + UINT16_T position_p_gain; + UINT16_T velocity_d_gain; + UINT16_T velocity_i_gain; + UINT16_T velocity_p_gain; + double goal_position; + double goal_velocity; + double goal_torque; + double present_position; + double present_velocity; + double present_load; + bool is_moving; + + UINT16_T ext_port_data[4]; + + double position_offset; + + DynamixelState() + : update_time_stamp(0, 0), + torque_enable(false), + position_d_gain(0), + position_i_gain(0), + position_p_gain(0), + velocity_d_gain(0), + velocity_i_gain(0), + velocity_p_gain(0), + goal_position(0.0), + goal_velocity(0.0), + goal_torque(0.0), + present_position(0.0), + present_velocity(0.0), + present_load(0.0), + is_moving(false), + position_offset(0.0) + { + ext_port_data[0] = 2048; + ext_port_data[1] = 2048; + ext_port_data[2] = 2048; + ext_port_data[3] = 2048; + } +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ */ diff --git a/robotis_device/include/robotis_device/Robot.h b/robotis_device/include/robotis_device/Robot.h new file mode 100644 index 0000000..121d385 --- /dev/null +++ b/robotis_device/include/robotis_device/Robot.h @@ -0,0 +1,35 @@ +/* + * Robot.h + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ + + +#include +#include "Sensor.h" +#include "Dynamixel.h" +#include "dynamixel_sdk/PortHandler.h" + +namespace ROBOTIS +{ + +class Robot +{ +public: + std::map ports; // string: port name + std::map dxls; // string: joint name + std::map sensors; // string: sensor name + + Robot(std::string robot_file_path, std::string dev_desc_dir_path); + + Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_ROBOT_H_ */ diff --git a/robotis_device/include/robotis_device/Sensor.h b/robotis_device/include/robotis_device/Sensor.h new file mode 100644 index 0000000..ca3e28a --- /dev/null +++ b/robotis_device/include/robotis_device/Sensor.h @@ -0,0 +1,34 @@ +/* + * Sensor.h + * + * Created on: 2015. 12. 16. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ + +#include +#include +#include +#include "ControlTableItem.h" + +namespace ROBOTIS +{ + +class Sensor +{ +public: + UINT8_T id; + std::string model_name; + float protocol_version; + + std::map ctrl_table; + + Sensor(int id, std::string model_name, float protocol_version); +}; + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ */ diff --git a/robotis_device/package.xml b/robotis_device/package.xml new file mode 100644 index 0000000..06de7e5 --- /dev/null +++ b/robotis_device/package.xml @@ -0,0 +1,34 @@ + + + robotis_device + 0.1.0 + The robotis_device package + + robotis + + + GPLv2 + + + + + + robotis + + + catkin + + roscpp + rospy + dynamixel_sdk + robotis_framework_common + + roscpp + rospy + dynamixel_sdk + + + + + + \ No newline at end of file diff --git a/robotis_device/src/robotis_device/Dynamixel.cpp b/robotis_device/src/robotis_device/Dynamixel.cpp new file mode 100644 index 0000000..baafc2e --- /dev/null +++ b/robotis_device/src/robotis_device/Dynamixel.cpp @@ -0,0 +1,37 @@ +/* + * Dynamixel.cpp + * + * Created on: 2015. 12. 8. + * Author: zerom + */ + +#include "../include/robotis_device/Dynamixel.h" + +using namespace ROBOTIS; + +Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version) + : id(id), + model_name(model_name), + port_name(""), + ctrl_module_name("none"), + protocol_version(protocol_version), + zero_radian_position_value(0), + min_radian_position_value(0), + max_radian_position_value(0), + min_radian(-3.14), + max_radian(3.14), + torque_enable_address(0), + position_d_gain_address(0), + position_i_gain_address(0), + position_p_gain_address(0), + goal_position_address(0), + goal_velocity_address(0), + goal_torque_address(0), + present_position_address(0), + present_velocity_address(0), + present_load_address(0), + is_moving_address(0) +{ + ctrl_table.clear(); + dxl_state = new DynamixelState(); +} diff --git a/robotis_device/src/robotis_device/Robot.cpp b/robotis_device/src/robotis_device/Robot.cpp new file mode 100644 index 0000000..43d2cb5 --- /dev/null +++ b/robotis_device/src/robotis_device/Robot.cpp @@ -0,0 +1,227 @@ +/* + * Robot.cpp + * + * Created on: 2015. 12. 11. + * Author: zerom + */ + +#include +#include +#include +#include "../include/robotis_device/Robot.h" + +using namespace ROBOTIS; + +static inline std::string <rim(std::string &s) { + s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); + return s; +} +static inline std::string &rtrim(std::string &s) { + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + return s; +} +static inline std::string &trim(std::string &s) { + return ltrim(rtrim(s)); +} + +static inline std::vector split(const std::string &text, char sep) { + std::vector tokens; + std::size_t start = 0, end = 0; + while((end = text.find(sep, start)) != (std::string::npos)) { + tokens.push_back(text.substr(start, end - start)); + trim(tokens.back()); + start = end + 1; + } + tokens.push_back(text.substr(start)); + trim(tokens.back()); + return tokens; +} + +Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path) +{ + if(dev_desc_dir_path.compare(dev_desc_dir_path.size()-1, 1, "/") != 0) + dev_desc_dir_path += "/"; + + std::ifstream file(robot_file_path.c_str()); + if(file.is_open()) + { + std::string session = ""; + std::string input_str; + while(!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if(pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + + // find session + if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size()-2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if(session == "port info") + { + std::vector tokens = split(input_str, '|'); + if(tokens.size() != 2) + continue; + + std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl; + + ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str()); + bool _port_result = ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str())); + if(_port_result == false) + exit(-1); + } + else if(session == "device info") + { + std::vector tokens = split(input_str, '|'); + if(tokens.size() != 6) + continue; + + if(tokens[0] == "dynamixel") + { + std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device"; + int _id = std::atoi(tokens[2].c_str()); + std::string _port = tokens[1]; + float _protocol = std::atof(tokens[4].c_str()); + std::string _dev_name = tokens[5]; + + dxls[_dev_name] = getDynamixel(_file_path, _id, _port, _protocol); + } + } + } + file.close(); + } + else + { + std::cout << "Unable to open file : " + robot_file_path << std::endl; + } +} + +Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version) +{ + Dynamixel *dxl; + // load file model_name.device + std::ifstream file(path.c_str()); + if(file.is_open()) + { + std::string session = ""; + std::string input_str; + while(!file.eof()) + { + std::getline(file, input_str); + + // remove comment ( # ) + std::size_t pos = input_str.find("#"); + if(pos != std::string::npos) + input_str = input_str.substr(0, pos); + + // trim + input_str = trim(input_str); + if(input_str == "") + continue; + + // find session + if(!input_str.compare(0, 1, "[") && !input_str.compare(input_str.size()-1, 1, "]")) + { + input_str = input_str.substr(1, input_str.size()-2); + std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower); + session = trim(input_str); + continue; + } + + if(session == "device info") + { + std::vector tokens = split(input_str, '='); + if(tokens.size() != 2) + continue; + + if(tokens[0] == "model_name") + dxl = new Dynamixel(id, tokens[1], protocol_version); + } + else if(session == "type info") + { + std::vector tokens = split(input_str, '='); + if(tokens.size() != 2) + continue; + + if(tokens[0] == "0_radian_position_value") + dxl->zero_radian_position_value = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "min_radian_position_value") + dxl->min_radian_position_value = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "max_radian_position_value") + dxl->max_radian_position_value = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "min_radian") + dxl->min_radian = std::atof(tokens[1].c_str()); + else if(tokens[0] == "max_radian") + dxl->max_radian = std::atof(tokens[1].c_str()); + else if(tokens[0] == "torque_enable_address") + dxl->torque_enable_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "position_d_gain_address") + dxl->position_d_gain_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "position_i_gain_address") + dxl->position_i_gain_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "position_p_gain_address") + dxl->position_p_gain_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "goal_position_address") + dxl->goal_position_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "goal_velocity_address") + dxl->goal_velocity_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "goal_torque_address") + dxl->goal_torque_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "present_position_address") + dxl->present_position_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "present_velocity_address") + dxl->present_velocity_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "present_load_address") + dxl->present_load_address = std::atoi(tokens[1].c_str()); + else if(tokens[0] == "is_moving_address") + dxl->is_moving_address = std::atoi(tokens[1].c_str()); + } + else if(session == "control table") + { + std::vector tokens = split(input_str, '|'); + if(tokens.size() != 8) + continue; + + ControlTableItem *item = new ControlTableItem(); + item->address = std::atoi(tokens[0].c_str()); + item->data_length = std::atoi(tokens[2].c_str()); + if(tokens[3] == "R") + item->access_type = READ; + else if(tokens[3] == "RW") + item->access_type = READ_WRITE; + if(tokens[4] == "EEPROM") + item->memory_type = EEPROM; + else if(tokens[4] == "RAM") + item->memory_type = RAM; + item->data_min_value = std::atol(tokens[5].c_str()); + item->data_max_value = std::atol(tokens[6].c_str()); + if(tokens[7] == "Y") + item->is_signed = true; + else if(tokens[7] == "N") + item->is_signed = false; + dxl->ctrl_table[tokens[1]] = item; + } + } + dxl->port_name = port; + + fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id, dxl->model_name.c_str()); + //std::cout << "[ID:" << (int)(dxl->id) << "] " << dxl->model_name << " added. (" << port << ")" << std::endl; + file.close(); + } + else + std::cout << "Unable to open file : " + path << std::endl; + + return dxl; +} + diff --git a/robotis_framework_common/CMakeLists.txt b/robotis_framework_common/CMakeLists.txt new file mode 100644 index 0000000..2a38558 --- /dev/null +++ b/robotis_framework_common/CMakeLists.txt @@ -0,0 +1,186 @@ +cmake_minimum_required(VERSION 2.8.3) +project(robotis_framework_common) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES robotis_framework_common +# CATKIN_DEPENDS roscpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(robotis_framework_common +# src/${PROJECT_NAME}/robotis_framework_common.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(robotis_framework_common ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(robotis_framework_common_node src/robotis_framework_common_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(robotis_framework_common_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(robotis_framework_common_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS robotis_framework_common robotis_framework_common_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_robotis_framework_common.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/robotis_framework_common/include/robotis_framework_common/MotionModule.h b/robotis_framework_common/include/robotis_framework_common/MotionModule.h new file mode 100644 index 0000000..8237032 --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/MotionModule.h @@ -0,0 +1,53 @@ +/* + * MotionModule.h + * + * Created on: 2016. 1. 15. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_MSGS_INCLUDE_MOTIONMODULE_H_ +#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_MSGS_INCLUDE_MOTIONMODULE_H_ + + +#include +#include + +#include "robotis_device/Dynamixel.h" +#include "robotis_device/DynamixelState.h" + +namespace ROBOTIS +{ + +enum CONTROL_MODE +{ + POSITION_CONTROL, + VELOCITY_CONTROL, + TORQUE_CONTROL +}; + +class MotionModule +{ +public: + bool enable; + std::string module_name; + CONTROL_MODE control_mode; + + std::map result; + + virtual ~MotionModule() { } + + virtual void Initialize(const int control_cycle_msec) = 0; + virtual void Process(std::map dxls) = 0; + + inline double powDI(double a, int b); +}; + +inline double MotionModule::powDI(double a, int b) +{ + return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); +} + +} + + +#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_MSGS_INCLUDE_MOTIONMODULE_H_ */ diff --git a/robotis_framework_common/include/robotis_framework_common/RobotisDef.h b/robotis_framework_common/include/robotis_framework_common/RobotisDef.h new file mode 100644 index 0000000..a860e9d --- /dev/null +++ b/robotis_framework_common/include/robotis_framework_common/RobotisDef.h @@ -0,0 +1,21 @@ +/* + * RobotisDef.h + * + * Created on: 2016. 1. 27. + * Author: zerom + */ + +#ifndef ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ +#define ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ + + +typedef char INT8_T; +typedef short int INT16_T; +typedef int INT32_T; + +typedef unsigned char UINT8_T; +typedef unsigned short int UINT16_T; +typedef unsigned int UINT32_T; + + +#endif /* ROBOTIS_FRAMEWORK_DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ROBOTISDEF_H_ */ diff --git a/robotis_framework_common/package.xml b/robotis_framework_common/package.xml new file mode 100644 index 0000000..7863f09 --- /dev/null +++ b/robotis_framework_common/package.xml @@ -0,0 +1,52 @@ + + + robotis_framework_common + 0.0.0 + The robotis_framework_common package + + + + + robotis + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + + + + + + + + \ No newline at end of file