Modified & Added sync functions. (#23)

Author: Kei

examples/advanced/sync_read_write_raw/sync_read_write_raw.ino

@@ -65,9 +65,9 @@ typedef struct sr_data{
   int32_t present_velocity;
   int32_t present_position;
 } sr_data_t;
-uint8_t id_list[DXL_ID_CNT] = {1, 3};
+uint8_t id_list[DXL_ID_CNT] = {1, 2};
 sr_data_t present_values[DXL_ID_CNT];
-int32_t goal_velocity[DXL_ID_CNT] = {100, 300};
+int32_t goal_velocity[DXL_ID_CNT] = {100, 200};
 
 Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
 
@@ -88,21 +88,18 @@ void setup() {
   // Generate SyncRead packet using user buffer
   sr_present_pos.p_packet_buf = user_pkt_buf;
   sr_present_pos.packet_buf_capacity = user_pkt_buf_cap;
-  dxl.beginSyncRead(SR_START_ADDR, SR_ADDR_LEN, &sr_present_pos);
+  dxl.beginSetupSyncRead(SR_START_ADDR, SR_ADDR_LEN, &sr_present_pos);
   for(i=0; i<DXL_ID_CNT; i++){
-    if(i<(DXL_ID_CNT-1)){
-      dxl.addSyncReadID(id_list[i], &sr_present_pos);
-    }else{
-      //3rd parameter is a flag to finish adding parameter (ID) and create packet.(default is false)
-      dxl.addSyncReadID(id_list[i], &sr_present_pos, true);
-    }
+    dxl.addSyncReadID(id_list[i], &sr_present_pos);
   }
+  dxl.endSetupSyncRead(&sr_present_pos);
 
   // Generate SyncRead packet using class default buffer
-  dxl.beginSyncWrite(SW_START_ADDR, SW_ADDR_LEN);
+  dxl.beginSetupSyncWrite(SW_START_ADDR, SW_ADDR_LEN);
   for(i=0; i<DXL_ID_CNT; i++){
     dxl.addSyncWriteData(id_list[i], (uint8_t*)&goal_velocity[i]);
   }
+  dxl.endSetupSyncWrite();
   dxl.sendSyncWrite();
 }
 

src/utility/master.cpp

@@ -531,7 +531,7 @@ Master::clear(uint8_t id, uint8_t option, uint32_t ex_option, uint32_t timeout_m
 // (Protocol 1.0) Not Supported
 // (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#sync-read
 bool 
-Master::beginSyncRead(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info)
+Master::beginSetupSyncRead(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info)
 {
   bool ret = false;
   DXLLibErrorCode_t err = DXL_LIB_OK;
@@ -574,7 +574,7 @@ Master::beginSyncRead(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sy
 }
 
 bool
-Master::addSyncReadID(uint8_t id, InfoSyncBulkInst_t *p_sync_info, bool flag_end_add)
+Master::addSyncReadID(uint8_t id, InfoSyncBulkInst_t *p_sync_info)
 {
   bool ret = false;
   DXLLibErrorCode_t err = DXL_LIB_OK;
@@ -599,23 +599,48 @@ Master::addSyncReadID(uint8_t id, InfoSyncBulkInst_t *p_sync_info, bool flag_end
 
   if(err == DXL_LIB_OK){
     p_info->id_cnt++;
-    if(flag_end_add == true){
-      err = end_make_dxl_packet(&info_tx_packet_);
-      if(err == DXL_LIB_OK){
-        p_info->packet_length = info_tx_packet_.generated_packet_length;
-        p_info->is_complete_packet = true;        
-        ret = true;
-      }
-    }else{
-      ret = true;
-    }
+    ret = true;
   }
 
   last_lib_err_ = err;
 
   return ret;
 }
 
+bool
+Master::endSetupSyncRead(InfoSyncBulkInst_t *p_sync_info)
+{
+  bool ret = false;
+  DXLLibErrorCode_t err = DXL_LIB_OK;
+  InfoSyncBulkInst_t *p_info = &info_sync_bulk_;
+
+  // Parameter exception handling
+  if(protocol_ver_idx_ != 2){
+    err = DXL_LIB_ERROR_NOT_SUPPORTED;
+  }else if(info_tx_packet_.inst_idx != DXL_INST_SYNC_READ){
+    err = DXL_LIB_ERROR_NOT_INITIALIZED;
+  }
+  if(err != DXL_LIB_OK){
+    last_lib_err_ = err;
+    return false;
+  }
+
+  if(p_sync_info != nullptr){
+    p_info = p_sync_info;
+  }
+
+  if(info_tx_packet_.inst_idx == DXL_INST_SYNC_READ){
+    err = end_make_dxl_packet(&info_tx_packet_);
+    if(err == DXL_LIB_OK){
+      p_info->packet_length = info_tx_packet_.generated_packet_length;
+      p_info->is_complete_packet = true;
+      ret = true;
+    }
+  }
+
+  return ret;  
+}
+
 uint8_t
 Master::sendSyncRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, InfoSyncBulkInst_t *p_sync_info)
 {
@@ -663,6 +688,8 @@ Master::sendSyncRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, InfoSyncBu
     if(rxStatusPacket(&p_recv_buf[i*p_info->addr_len], 
     recv_buf_capacity-i*p_info->addr_len, 3) != nullptr){
       recv_cnt++;
+    }else{
+      break;
     }
   }
 
@@ -674,7 +701,7 @@ Master::sendSyncRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, InfoSyncBu
 // (Protocol 1.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol1/#sync-write
 // (Protocol 2.0) Refer to http://emanual.robotis.com/docs/en/dxl/protocol2/#sync-write
 bool 
-Master::beginSyncWrite(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info)
+Master::beginSetupSyncWrite(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info)
 {
   bool ret = false;
   DXLLibErrorCode_t err = DXL_LIB_OK;
@@ -717,7 +744,7 @@ Master::beginSyncWrite(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_s
 }
 
 bool
-Master::addSyncWriteData(uint8_t id, uint8_t *p_data, InfoSyncBulkInst_t *p_sync_info, bool flag_end_add)
+Master::addSyncWriteData(uint8_t id, uint8_t *p_data, InfoSyncBulkInst_t *p_sync_info)
 {
   bool ret = false;
   DXLLibErrorCode_t err = DXL_LIB_OK;
@@ -743,16 +770,7 @@ Master::addSyncWriteData(uint8_t id, uint8_t *p_data, InfoSyncBulkInst_t *p_sync
     err = add_param_to_dxl_packet(&info_tx_packet_, (uint8_t*)p_data, p_info->addr_len);
     if(err == DXL_LIB_OK){
       p_info->id_cnt++;
-      if(flag_end_add == true){
-        err = end_make_dxl_packet(&info_tx_packet_);
-        if(err == DXL_LIB_OK){
-          p_info->packet_length = info_tx_packet_.generated_packet_length;
-          p_info->is_complete_packet = true;        
-          ret = true;
-        }
-      }else{
-        ret = true;
-      }
+      ret = true;
     }
   }
 
@@ -761,6 +779,40 @@ Master::addSyncWriteData(uint8_t id, uint8_t *p_data, InfoSyncBulkInst_t *p_sync
   return ret;
 }
 
+bool
+Master::endSetupSyncWrite(InfoSyncBulkInst_t *p_sync_info)
+{
+  bool ret = false;
+  DXLLibErrorCode_t err = DXL_LIB_OK;
+  InfoSyncBulkInst_t *p_info = &info_sync_bulk_;
+
+  // Parameter exception handling
+  if(protocol_ver_idx_ != 2){
+    err = DXL_LIB_ERROR_NOT_SUPPORTED;
+  }else if(info_tx_packet_.inst_idx != DXL_INST_SYNC_WRITE){
+    err = DXL_LIB_ERROR_NOT_INITIALIZED;
+  }
+  if(err != DXL_LIB_OK){
+    last_lib_err_ = err;
+    return false;
+  }
+
+  if(p_sync_info != nullptr){
+    p_info = p_sync_info;
+  }
+
+  if(info_tx_packet_.inst_idx == DXL_INST_SYNC_WRITE){
+    err = end_make_dxl_packet(&info_tx_packet_);
+    if(err == DXL_LIB_OK){
+      p_info->packet_length = info_tx_packet_.generated_packet_length;
+      p_info->is_complete_packet = true;
+      ret = true;
+    }
+  }
+
+  return ret;
+}
+
 bool
 Master::sendSyncWrite(InfoSyncBulkInst_t *p_sync_info)
 {

src/utility/master.h

@@ -109,13 +109,15 @@ class Master
     // bool bulkWrite(uint8_t *p_param, uint16_t param_len, uint32_t timeout_ms = 10);
 
     /* Easy functions for Sync Read */
-    bool beginSyncRead(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info = nullptr);
-    bool addSyncReadID(uint8_t id, InfoSyncBulkInst_t *p_sync_info = nullptr, bool flag_end_add = false);
+    bool beginSetupSyncRead(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info = nullptr);
+    bool addSyncReadID(uint8_t id, InfoSyncBulkInst_t *p_sync_info = nullptr);
+    bool endSetupSyncRead(InfoSyncBulkInst_t *p_sync_info = nullptr);
     uint8_t sendSyncRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, InfoSyncBulkInst_t *p_sync_info = nullptr);
 
     /* Easy functions for Sync Write */
-    bool beginSyncWrite(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info = nullptr);
-    bool addSyncWriteData(uint8_t id, uint8_t *p_data, InfoSyncBulkInst_t *p_sync_info = nullptr, bool flag_end_add = false);
+    bool beginSetupSyncWrite(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info = nullptr);
+    bool addSyncWriteData(uint8_t id, uint8_t *p_data, InfoSyncBulkInst_t *p_sync_info = nullptr);
+    bool endSetupSyncWrite(InfoSyncBulkInst_t *p_sync_info = nullptr);
     bool sendSyncWrite(InfoSyncBulkInst_t *p_sync_info = nullptr);
 
     /* Easy functions for Bulk Read */