.. _program_listing_file__tmp_ws_src_dynamixel_sdk_dynamixel_sdk_include_dynamixel_sdk_packet_handler.h: Program Listing for File packet_handler.h ========================================= |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/dynamixel_sdk/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /******************************************************************************* * Copyright 2017 ROBOTIS CO., LTD. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. *******************************************************************************/ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ #if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include #define ERROR_PRINT SerialBT2.print #else #define ERROR_PRINT printf #endif #include #include #include "port_handler.h" #define BROADCAST_ID 0xFE // 254 #define MAX_ID 0xFC // 252 /* Macro for Control Table Value */ #define DXL_MAKEWORD(a, b) ((uint16_t)(((uint8_t)(((uint64_t)(a)) & 0xff)) | ((uint16_t)((uint8_t)(((uint64_t)(b)) & 0xff))) << 8)) #define DXL_MAKEDWORD(a, b) ((uint32_t)(((uint16_t)(((uint64_t)(a)) & 0xffff)) | ((uint32_t)((uint16_t)(((uint64_t)(b)) & 0xffff))) << 16)) #define DXL_LOWORD(l) ((uint16_t)(((uint64_t)(l)) & 0xffff)) #define DXL_HIWORD(l) ((uint16_t)((((uint64_t)(l)) >> 16) & 0xffff)) #define DXL_LOBYTE(w) ((uint8_t)(((uint64_t)(w)) & 0xff)) #define DXL_HIBYTE(w) ((uint8_t)((((uint64_t)(w)) >> 8) & 0xff)) /* Instruction for DXL Protocol */ #define INST_PING 1 #define INST_READ 2 #define INST_WRITE 3 #define INST_REG_WRITE 4 #define INST_ACTION 5 #define INST_FACTORY_RESET 6 #define INST_SYNC_WRITE 131 // 0x83 #define INST_BULK_READ 146 // 0x92 // --- Only for 2.0 --- // #define INST_REBOOT 8 #define INST_CLEAR 16 // 0x10 #define INST_STATUS 85 // 0x55 #define INST_SYNC_READ 130 // 0x82 #define INST_BULK_WRITE 147 // 0x93 // Communication Result #define COMM_SUCCESS 0 // tx or rx packet communication success #define COMM_PORT_BUSY -1000 // Port is busy (in use) #define COMM_TX_FAIL -1001 // Failed transmit instruction packet #define COMM_RX_FAIL -1002 // Failed get status packet #define COMM_TX_ERROR -2000 // Incorrect instruction packet #define COMM_RX_WAITING -3000 // Now recieving status packet #define COMM_RX_TIMEOUT -3001 // There is no status packet #define COMM_RX_CORRUPT -3002 // Incorrect status packet #define COMM_NOT_AVAILABLE -9000 // namespace dynamixel { class WINDECLSPEC PacketHandler { protected: PacketHandler() { } public: static PacketHandler *getPacketHandler(float protocol_version = 2.0); virtual ~PacketHandler() { } virtual float getProtocolVersion() = 0; virtual const char *getTxRxResult (int result) = 0; virtual const char *getRxPacketError (uint8_t error) = 0; virtual int txPacket (PortHandler *port, uint8_t *txpacket) = 0; virtual int rxPacket (PortHandler *port, uint8_t *rxpacket) = 0; virtual int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) = 0; virtual int ping (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; virtual int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0; 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 clearMultiTurn (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, uint8_t id, 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 id, 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, uint8_t id, 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, uint8_t id, uint32_t *data, uint8_t *error = 0) = 0; virtual int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0) = 0; virtual int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; virtual int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) = 0; virtual int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0; virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) = 0; virtual int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0; virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) = 0; virtual int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0; virtual int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; virtual int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; virtual int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; // SyncReadRx -> GroupSyncRead class // SyncReadTxRx -> GroupSyncRead class virtual int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; virtual int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; // BulkReadRx -> GroupBulkRead class // BulkReadTxRx -> GroupBulkRead class virtual int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; }; } #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */