diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h index 70fd70dd96dd7b9a96ddd3fd877a9ad4916e3d9b..64df2a8516c1973238b95bf8921815cd8e94939b 100644 --- a/dynamixel_base/include/dynamixel_master.h +++ b/dynamixel_base/include/dynamixel_master.h @@ -214,7 +214,7 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u * \brief * */ -unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt); +unsigned char dyn_master_relay(TDynamixelMaster *master,TDynVersion inst_ver,unsigned char *inst_pkt,unsigned char *status_pkt); #ifdef __cplusplus } diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c index a91820b2c85dee635a94337860956ec78d5335e6..0fb6a964ca20c8823c3b6e3c997771e3e763238a 100644 --- a/dynamixel_base/src/dynamixel_master.c +++ b/dynamixel_base/src/dynamixel_master.c @@ -1297,13 +1297,26 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u return error; } -unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt) +unsigned char dyn_master_relay(TDynamixelMaster *master,TDynVersion inst_ver,unsigned char *inst_pkt,unsigned char *status_pkt) { - unsigned char error=DYN_COMM_ERROR; + unsigned char error=DYN_COMM_ERROR,i; if(master!=0x00000000) { - dyn2_copy_packet(inst_pkt,master->tx_buffer); + if(inst_ver==DYN_VER1) + { + if(master->version==DYN_VER1) + dyn_copy_packet(inst_pkt,master->tx_buffer); + else + dyn2_convert_v1_inst_packet(inst_pkt,master->tx_buffer); + } + else + { + if(master->version==DYN_VER1) + dyn_convert_v2_inst_packet(inst_pkt,master->tx_buffer); + else + dyn2_copy_packet(inst_pkt,master->tx_buffer); + } master->rx_num_packets=0x01; master->rx_no_answer=0x00; // enable transmission @@ -1322,7 +1335,20 @@ unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt, } // wait for the replay within the given timeout error=dyn_master_wait_reception(master); - dyn2_copy_packet(master->rx_buffer,status_pkt); + if(inst_ver==DYN_VER1) + { + if(master->version==DYN_VER1) + dyn_copy_packet(master->rx_buffer,status_pkt); + else + dyn_convert_v2_status_packet(master->rx_buffer,status_pkt); + } + else + { + if(master->version==DYN_VER1) + dyn2_convert_v1_status_packet((TDynInstruction)inst_pkt[DYN2_INST_OFF],master->rx_buffer,status_pkt); + else + dyn2_copy_packet(master->rx_buffer,status_pkt); + } } return error;