Skip to content

Commit

Permalink
Merge branch 'kinetic-devel' of github.com:ROBOTIS-GIT/DynamixelSDK i…
Browse files Browse the repository at this point in the history 
…nto kinetic-devel
  • Loading branch information
@robotpilot
robotpilot committed May 23, 2017
2 parents 5dc0ec9 + 3fd7650 commit dd948da
Show file tree
Hide file tree
Showing 7 changed files with 99 additions and 89 deletions.
8 changes: 4 additions & 4 deletions ros/include/dynamixel_sdk/packet_handler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
24 changes: 12 additions & 12 deletions ros/include/dynamixel_sdk/protocol1_packet_handler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
24 changes: 12 additions & 12 deletions ros/include/dynamixel_sdk/protocol2_packet_handler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion ros/src/group_bulk_read.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion ros/src/group_sync_read.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
88 changes: 47 additions & 41 deletions ros/src/protocol1_packet_handler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;
}
Expand Down
40 changes: 22 additions & 18 deletions ros/src/protocol2_packet_handler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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];
Expand Down Expand Up @@ -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];
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand Down

0 comments on commit dd948da

Please sign in to comment.