diff --git a/ros/include/dynamixel_sdk/packet_handler.h b/ros/include/dynamixel_sdk/packet_handler.h index e1ddfd3f..a3de440f 100644 --- a/ros/include/dynamixel_sdk/packet_handler.h +++ b/ros/include/dynamixel_sdk/packet_handler.h @@ -109,19 +109,19 @@ class WINDECLSPEC PacketHandler 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 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 *data, uint8_t *error = 0) = 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, uint16_t *data, uint8_t *error = 0) = 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, uint32_t *data, uint8_t *error = 0) = 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; diff --git a/ros/include/dynamixel_sdk/protocol1_packet_handler.h b/ros/include/dynamixel_sdk/protocol1_packet_handler.h index 11d650b5..d4ae3f16 100644 --- a/ros/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/ros/include/dynamixel_sdk/protocol1_packet_handler.h @@ -72,35 +72,35 @@ class WINDECLSPEC Protocol1PacketHandler : public PacketHandler 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 readRx (PortHandler *port, uint8_t id, 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 read1ByteRx (PortHandler *port, uint8_t id, 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 read2ByteRx (PortHandler *port, uint8_t id, 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 read4ByteRx (PortHandler *port, uint8_t id, 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 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 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 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 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 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 diff --git a/ros/include/dynamixel_sdk/protocol2_packet_handler.h b/ros/include/dynamixel_sdk/protocol2_packet_handler.h index b6c26eda..507b6f4a 100644 --- a/ros/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/ros/include/dynamixel_sdk/protocol2_packet_handler.h @@ -76,35 +76,35 @@ class WINDECLSPEC Protocol2PacketHandler : public PacketHandler 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 readRx (PortHandler *port, uint8_t id, 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 read1ByteRx (PortHandler *port, uint8_t id, 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 read2ByteRx (PortHandler *port, uint8_t id, 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 read4ByteRx (PortHandler *port, uint8_t id, 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 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 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 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 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 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 diff --git a/ros/src/group_bulk_read.cpp b/ros/src/group_bulk_read.cpp index 466cebc5..05f7c448 100644 --- a/ros/src/group_bulk_read.cpp +++ b/ros/src/group_bulk_read.cpp @@ -167,7 +167,7 @@ int GroupBulkRead::rxPacket() { uint8_t id = id_list_[i]; - result = ph_->readRx(port_, length_list_[id], data_list_[id]); + result = ph_->readRx(port_, id, length_list_[id], data_list_[id]); if (result != COMM_SUCCESS) return result; } diff --git a/ros/src/group_sync_read.cpp b/ros/src/group_sync_read.cpp index a3cac9ab..62e4275b 100644 --- a/ros/src/group_sync_read.cpp +++ b/ros/src/group_sync_read.cpp @@ -139,7 +139,7 @@ int GroupSyncRead::rxPacket() { uint8_t id = id_list_[i]; - result = ph_->readRx(port_, data_length_, data_list_[id]); + result = ph_->readRx(port_, id, data_length_, data_list_[id]); if (result != COMM_SUCCESS) return result; } diff --git a/ros/src/protocol1_packet_handler.cpp b/ros/src/protocol1_packet_handler.cpp index 68d22db0..e8a7d362 100644 --- a/ros/src/protocol1_packet_handler.cpp +++ b/ros/src/protocol1_packet_handler.cpp @@ -286,44 +286,47 @@ int Protocol1PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) // NOT for BulkRead instruction int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - // tx packet - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - return result; + // 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; - } + // (Instruction == BulkRead) == this function is not available. + if(txpacket[PKT_INSTRUCTION] == INST_BULK_READ) + result = COMM_NOT_AVAILABLE; - // set packet timeout - if (txpacket[PKT_INSTRUCTION] == INST_READ) - { - port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6)); - } - else - { - port->setPacketTimeout((uint16_t)6); - } + // (ID == Broadcast ID) == no need to wait for status packet or not available + // (Instruction == action) == no need to wait for status packet + if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION) + { + port->is_using_ = false; + return result; + } + + // set packet timeout + if (txpacket[PKT_INSTRUCTION] == INST_READ) + { + port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6)); + } + else + { + port->setPacketTimeout((uint16_t)6); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + } - // rx packet + // rx packet + do { result = rxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if (txpacket[PKT_ID] != rxpacket[PKT_ID]) - result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]); - if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - return result; + if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) + { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + + return result; } int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) @@ -413,14 +416,17 @@ int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre return result; } -int Protocol1PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { - int result = COMM_TX_FAIL; + 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) + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); + + if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { if (error != 0) { @@ -477,10 +483,10 @@ int Protocol1PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t { return readTx(port, id, address, 1); } -int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) +int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error) { uint8_t data_read[1] = {0}; - int result = readRx(port, 1, data_read, error); + int result = readRx(port, id, 1, data_read, error); if (result == COMM_SUCCESS) *data = data_read[0]; return result; @@ -498,10 +504,10 @@ int Protocol1PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t { return readTx(port, id, address, 2); } -int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) +int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error) { uint8_t data_read[2] = {0}; - int result = readRx(port, 2, data_read, error); + int result = readRx(port, id, 2, data_read, error); if (result == COMM_SUCCESS) *data = DXL_MAKEWORD(data_read[0], data_read[1]); return result; @@ -519,7 +525,7 @@ int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t { return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) +int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error) { return COMM_NOT_AVAILABLE; } diff --git a/ros/src/protocol2_packet_handler.cpp b/ros/src/protocol2_packet_handler.cpp index 12ab01b3..279eafdf 100644 --- a/ros/src/protocol2_packet_handler.cpp +++ b/ros/src/protocol2_packet_handler.cpp @@ -437,11 +437,13 @@ int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin if (result != COMM_SUCCESS) return result; - // (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet + // (Instruction == BulkRead or SyncRead) == this function is not available. + if (txpacket[PKT_INSTRUCTION] == INST_BULK_READ || txpacket[PKT_INSTRUCTION] == INST_SYNC_READ) + result = COMM_NOT_AVAILABLE; + + // (ID == Broadcast ID) == no need to wait for status packet or not available. // (Instruction == action) == no need to wait for status packet - if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) || - (txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_SYNC_READ) || - (txpacket[PKT_INSTRUCTION] == INST_ACTION)) + if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION) { port->is_using_ = false; return result; @@ -459,12 +461,11 @@ int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uin } // rx packet - result = rxPacket(port, rxpacket); - // check txpacket ID == rxpacket ID - if (txpacket[PKT_ID] != rxpacket[PKT_ID]) + do { result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]); - if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID) + if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) { if (error != 0) *error = (uint8_t)rxpacket[PKT_ERROR]; @@ -660,15 +661,18 @@ int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t addre return result; } -int Protocol2PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length + 11 + (length/3)); // (length/3): consider stuffing //uint8_t *rxpacket = new uint8_t[length + 11 + (length/3)]; // (length/3): consider stuffing - result = rxPacket(port, rxpacket); - if (result == COMM_SUCCESS) + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); + + if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { if (error != 0) *error = (uint8_t)rxpacket[PKT_ERROR]; @@ -721,10 +725,10 @@ int Protocol2PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t { return readTx(port, id, address, 1); } -int Protocol2PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error) +int Protocol2PacketHandler::read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error) { uint8_t data_read[1] = {0}; - int result = readRx(port, 1, data_read, error); + int result = readRx(port, id, 1, data_read, error); if (result == COMM_SUCCESS) *data = data_read[0]; return result; @@ -742,10 +746,10 @@ int Protocol2PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t { return readTx(port, id, address, 2); } -int Protocol2PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error) +int Protocol2PacketHandler::read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error) { uint8_t data_read[2] = {0}; - int result = readRx(port, 2, data_read, error); + int result = readRx(port, id, 2, data_read, error); if (result == COMM_SUCCESS) *data = DXL_MAKEWORD(data_read[0], data_read[1]); return result; @@ -763,10 +767,10 @@ int Protocol2PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t { return readTx(port, id, address, 4); } -int Protocol2PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error) +int Protocol2PacketHandler::read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error) { uint8_t data_read[4] = {0}; - int result = readRx(port, 4, data_read, error); + int result = readRx(port, id, 4, data_read, error); if (result == COMM_SUCCESS) *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); return result;