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(); }