diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c index da913fb6c151380940718ccaa88fa4996911650b..a91820b2c85dee635a94337860956ec78d5335e6 100644 --- a/dynamixel_base/src/dynamixel_master.c +++ b/dynamixel_base/src/dynamixel_master.c @@ -247,21 +247,6 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master) } master->packet_ready=0x00; return DYN_SUCCESS; - // check the input packet checksum - if(master->version==DYN_VER1) - { - if(dyn_check_checksum(master->rx_buffer)==0xFF) - return dyn_get_status_error(master->rx_buffer); - else - return DYN_CHECKSUM_ERROR; - } - else - { - if(dyn2_check_checksum(master->rx_buffer)==0x01) - return dyn2_get_status_error(master->rx_buffer); - else - return DYN_CHECKSUM_ERROR; - } } else return DYN_COMM_ERROR; @@ -326,17 +311,20 @@ unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data) { + static unsigned char tx_done=0; unsigned char error=DYN_COMM_ERROR; if(master!=0x00000000) { - if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) + if(tx_done==1 || (error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) { // wait for the replay within the given timeout if(master->return_level!=no_return && id!=DYN_BROADCAST_ID) { + tx_done=1; if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) { + tx_done=0; // check the input packet checksum if(master->version==DYN_VER1) { @@ -373,6 +361,11 @@ unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned ch return DYN_CHECKSUM_ERROR; } } + else if(error==DYN_TIMEOUT) + { + tx_done=0; + return error; + } else return error; } @@ -414,17 +407,20 @@ unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned cha unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data) { + static unsigned char tx_done=0; unsigned char error=DYN_COMM_ERROR; if(master!=0x00000000) { - if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) + if(tx_done==1 || (error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) { // wait for the replay within the given timeout if(master->return_level==return_all && id!=DYN_BROADCAST_ID) { + tx_done=1; if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) { + tx_done=0; // check the input packet checksum if(master->version==DYN_VER1) { @@ -447,6 +443,11 @@ unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned c else return DYN_SUCCESS; } + else if(error==DYN_TIMEOUT) + { + tx_done=0; + return error; + } else return error; } @@ -530,6 +531,7 @@ 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 tx_done=0; unsigned char error=DYN_COMM_ERROR,i; if(master!=0x00000000) @@ -537,13 +539,15 @@ unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned cha // generate the read packet for the desired device if(master->version==DYN_VER2) { - if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) + if(tx_done==1 || (error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) { // wait for the replay within the given timeout if(master->return_level!=no_return) { + tx_done=1; if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) { + tx_done=0; for(i=0;i<num;i++) { if(dyn2_check_checksum(&master->rx_buffer[(length+11)*i])==0x01) @@ -567,6 +571,11 @@ unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned cha else return DYN_SUCCESS; } + else if(error==DYN_TIMEOUT) + { + tx_done=0; + return error; + } else return error; } @@ -666,17 +675,20 @@ 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[]) { unsigned char error=DYN_COMM_ERROR,i; + static unsigned char tx_done=0; unsigned short int start=0; if(master!=0x00000000) { - if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) + if(tx_done==1 || (error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) { // wait for the replay within the given timeout if(master->return_level!=no_return) { + tx_done=1; if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) { + tx_done=0; for(i=0;i<num;i++) { // check the input packet checksum @@ -715,6 +727,11 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha } return DYN_SUCCESS; } + else if(error==DYN_TIMEOUT) + { + tx_done=0; + return error; + } else return error; }