From 817eaecb6611337612f0dbf1316ba6df9787120c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu> Date: Mon, 19 Oct 2015 16:43:12 +0000 Subject: [PATCH] Solved aproblem in the sync_Read and sync_write commands when one of the addressed devices does not respond in time. --- dynamixel_base/include/dynamixel.h | 1 + dynamixel_base/include/dynamixel2.h | 1 + dynamixel_base/src/dynamixel.c | 6 ++++++ dynamixel_base/src/dynamixel2.c | 5 +++++ dynamixel_base/src/dynamixel_master.c | 2 ++ 5 files changed, 15 insertions(+) diff --git a/dynamixel_base/include/dynamixel.h b/dynamixel_base/include/dynamixel.h index 8b73f9c..263e03d 100755 --- a/dynamixel_base/include/dynamixel.h +++ b/dynamixel_base/include/dynamixel.h @@ -72,5 +72,6 @@ void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError err inline TDynError dyn_get_status_error(unsigned char *packet); /* read instruction status packet */ unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data); +inline unsigned char dyn_get_read_status_id(unsigned char *packet); #endif diff --git a/dynamixel_base/include/dynamixel2.h b/dynamixel_base/include/dynamixel2.h index f51473c..a2d8e30 100644 --- a/dynamixel_base/include/dynamixel2.h +++ b/dynamixel_base/include/dynamixel2.h @@ -80,5 +80,6 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er inline TDynError dyn2_get_status_error(unsigned char *packet); /* read instruction status packet */ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char *data); +inline unsigned char dyn2_get_read_status_id(unsigned char *packet); #endif diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c index 8a72020..659c3cd 100755 --- a/dynamixel_base/src/dynamixel.c +++ b/dynamixel_base/src/dynamixel.c @@ -330,3 +330,9 @@ unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data return packet[DYN_LENGTH_OFF]-0x02; } + +inline unsigned char dyn_get_read_status_id(unsigned char *packet) +{ + return packet[DYN_ID_OFF]; +} + diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c index b85db07..85c6cec 100644 --- a/dynamixel_base/src/dynamixel2.c +++ b/dynamixel_base/src/dynamixel2.c @@ -483,3 +483,8 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char return length; } + +inline unsigned char dyn2_get_read_status_id(unsigned char *packet) +{ + return packet[DYN2_ID_OFF]; +} diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c index 292a25f..62e202c 100644 --- a/dynamixel_base/src/dynamixel_master.c +++ b/dynamixel_base/src/dynamixel_master.c @@ -519,6 +519,7 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un { if((error=dyn_master_wait_reception(master))==DYN_SUCCESS) { + while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++; if(dyn2_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length)// not enough data error=DYN_INST_ERROR; } @@ -578,6 +579,7 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un } else { + while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++; if(dyn2_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data error=DYN_INST_ERROR; } -- GitLab