diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 3c7f0470418d0303be47617347f68b0ed145fb4c..53345e783e670312770579249a82b04ce1571939 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -88,6 +88,16 @@ typedef struct
*
*/
unsigned char rx_num_packets;
+ /**
+ * \brief
+ *
+ */
+ unsigned char sync_num_dev_done;
+ /**
+ * \brief
+ *
+ */
+ unsigned char bulk_num_dev_done;
}TDynamixelMaster;
/* public functions */
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index 113992a75626b08ec216de072cc1ee5e21396037..bd61ade6dc3df8bd4ead424cbb43e0e3071522f5 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -502,7 +502,6 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char
unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[])
{
- static unsigned char num_dev_done=0;
unsigned char error=DYN_COMM_ERROR;
if(master!=0x00000000)
@@ -515,7 +514,7 @@ unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned cha
// wait for the replay within the given timeout
if(master->return_level!=no_return)
{
- if(num_dev_done<num)
+ if(master->sync_num_dev_done<num)
{
if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
{
@@ -523,24 +522,24 @@ unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned cha
{
if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
{
- while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++;
- if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[num_dev_done])!=length)// not enough data
+ while(dyn2_get_read_status_id(master->rx_buffer)!=ids[master->sync_num_dev_done]) master->sync_num_dev_done++;
+ if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[master->sync_num_dev_done])!=length)// not enough data
return DYN_INST_ERROR;
else
{
- num_dev_done++;
+ master->sync_num_dev_done++;
return DYN_BUSY;
}
}
else
{
- num_dev_done=0;
+ master->sync_num_dev_done=0;
return error;
}
}
else
{
- num_dev_done=0;
+ master->sync_num_dev_done=0;
return DYN_CHECKSUM_ERROR;
}
}
@@ -549,13 +548,13 @@ unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned cha
}
else
{
- num_dev_done=0;
+ master->sync_num_dev_done=0;
return DYN_SUCCESS;
}
}
else
{
- num_dev_done=0;
+ master->sync_num_dev_done=0;
return DYN_SUCCESS;
}
}
@@ -657,7 +656,6 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char
unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[])
{
- static unsigned char num_dev_done=0;
unsigned char error=DYN_COMM_ERROR;
if(master!=0x00000000)
@@ -667,7 +665,7 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha
// wait for the replay within the given timeout
if(master->return_level!=no_return)
{
- if(num_dev_done<num)
+ if(master->bulk_num_dev_done<num)
{
if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS)
{
@@ -678,24 +676,27 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha
{
if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS)
{
- while(dyn_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++;
- if(dyn_get_read_status_data(master->rx_buffer,(unsigned char *)data[num_dev_done])!=length[num_dev_done])// not enough data
+ std::cout << "RX id: " << (int)dyn_get_read_status_id(master->rx_buffer) << " num. dev: " << (int)master->bulk_num_dev_done << std::endl;
+ while(dyn_get_read_status_id(master->rx_buffer)!=ids[master->bulk_num_dev_done]) master->bulk_num_dev_done++;
+ if(dyn_get_read_status_data(master->rx_buffer,(unsigned char *)data[master->bulk_num_dev_done])!=length[master->bulk_num_dev_done])// not enough data
return DYN_INST_ERROR;
else
{
- num_dev_done++;
+ master->bulk_num_dev_done++;
return DYN_BUSY;
}
}
else
{
- num_dev_done=0;
+ std::cout << "error" << std::endl;
+ master->bulk_num_dev_done=0;
return error;
}
}
else
{
- num_dev_done++;
+ std::cout << "checksum error" << std::endl;
+ master->bulk_num_dev_done++;
return DYN_BUSY;
}
}
@@ -705,24 +706,24 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha
{
if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS)
{
- while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++;
- if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[num_dev_done])!=length[num_dev_done])// not enough data
+ while(dyn2_get_read_status_id(master->rx_buffer)!=ids[master->bulk_num_dev_done]) master->bulk_num_dev_done++;
+ if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[master->bulk_num_dev_done])!=length[master->bulk_num_dev_done])// not enough data
return DYN_INST_ERROR;
else
{
- num_dev_done++;
+ master->bulk_num_dev_done++;
return DYN_BUSY;
}
}
else
{
- num_dev_done=0;
+ master->bulk_num_dev_done=0;
return error;
}
}
else
{
- num_dev_done++;
+ master->bulk_num_dev_done++;
return DYN_BUSY;
}
}
@@ -732,13 +733,13 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha
}
else
{
- num_dev_done=0;
+ master->bulk_num_dev_done=0;
return DYN_SUCCESS;
}
}
else
{
- num_dev_done=0;
+ master->bulk_num_dev_done=0;
return DYN_SUCCESS;
}
}
@@ -769,6 +770,8 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
master->rx_no_answer=0x00;
master->rx_num_packets=0x00;
master->return_level=return_all;
+ master->sync_num_dev_done=0;
+ master->bulk_num_dev_done=0;
master->set_rx_mode();
}