Moved the static num_dev_done variable from the bulk and sync read done...

Moved the static num_dev_done variable from the bulk and sync read done functions to the master data structure.

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 */

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