Updated sync functions to return packet error. (#23)

Author: Kei

examples/advanced/sync_read_write_raw/sync_read_write_raw.ino

@@ -67,6 +67,7 @@ typedef struct sr_data{
 } sr_data_t;
 uint8_t id_list[DXL_ID_CNT] = {1, 2};
 sr_data_t present_values[DXL_ID_CNT];
+uint8_t err_list[DXL_ID_CNT] = {0, };
 int32_t goal_velocity[DXL_ID_CNT] = {100, 200};
 
 Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
@@ -100,19 +101,20 @@ void setup() {
     dxl.addSyncWriteData(id_list[i], (uint8_t*)&goal_velocity[i]);
   }
   dxl.endSetupSyncWrite();
-  dxl.sendSyncWrite();
+  dxl.doSyncWrite();
 }
 
 void loop() {
   // put your main code here, to run repeatedly:
   uint8_t recv_cnt;
 
-  recv_cnt = dxl.sendSyncRead((uint8_t*)&present_values, sizeof(present_values), &sr_present_pos);
+  recv_cnt = dxl.doSyncRead((uint8_t*)&present_values, sizeof(present_values), err_list, sizeof(err_list), &sr_present_pos);
   if(recv_cnt > 0){
     DEBUG_SERIAL.print("Received ID Count: ");
     DEBUG_SERIAL.println(recv_cnt);
     for(uint16_t i=0; i<recv_cnt; i++){
-      DEBUG_SERIAL.print("  ID: ");DEBUG_SERIAL.println(id_list[i]);
+      DEBUG_SERIAL.print("  ID: ");DEBUG_SERIAL.print(id_list[i]);
+      DEBUG_SERIAL.print(", Error: ");DEBUG_SERIAL.println(err_list[i]);
       DEBUG_SERIAL.print("\t Present Current: ");DEBUG_SERIAL.println(present_values[i].present_current);
       DEBUG_SERIAL.print("\t Present Velocity: ");DEBUG_SERIAL.println(present_values[i].present_velocity);
       DEBUG_SERIAL.print("\t Present Position: ");DEBUG_SERIAL.println(present_values[i].present_position);

src/utility/master.cpp

@@ -642,7 +642,7 @@ Master::endSetupSyncRead(InfoSyncBulkInst_t *p_sync_info)
 }
 
 uint8_t
-Master::sendSyncRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, InfoSyncBulkInst_t *p_sync_info)
+Master::doSyncRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, uint8_t *p_err_list, uint8_t err_list_size, InfoSyncBulkInst_t *p_sync_info)
 {
   uint8_t recv_cnt = 0;
   DXLLibErrorCode_t err = DXL_LIB_OK;
@@ -687,6 +687,9 @@ 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){
+      if(p_err_list != nullptr && err_list_size > recv_cnt){
+        p_err_list[recv_cnt] = info_rx_packet_.err_idx;
+      }
       recv_cnt++;
     }else{
       break;
@@ -814,7 +817,7 @@ Master::endSetupSyncWrite(InfoSyncBulkInst_t *p_sync_info)
 }
 
 bool
-Master::sendSyncWrite(InfoSyncBulkInst_t *p_sync_info)
+Master::doSyncWrite(InfoSyncBulkInst_t *p_sync_info)
 {
   bool ret = false;
   DXLLibErrorCode_t err = DXL_LIB_OK;

src/utility/master.h

@@ -112,23 +112,25 @@ class Master
     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);
+    uint8_t doSyncRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, uint8_t *p_err_list, uint8_t err_list_size, InfoSyncBulkInst_t *p_sync_info = nullptr);
 
     /* Easy functions for Sync Write */
     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);
+    bool doSyncWrite(InfoSyncBulkInst_t *p_sync_info = nullptr);
 
     /* Easy functions for Bulk Read */
-    bool beginBulkRead();
-    bool addBulkReadID(uint8_t id, uint16_t addr, uint16_t addr_len);
-    bool sendBulkRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity);
+    bool beginSetupBulkRead(InfoSyncBulkInst_t *p_bulk_info = nullptr);
+    bool addBulkReadID(uint8_t id, uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_bulk_info = nullptr);
+    bool endSetupBulkRead(InfoSyncBulkInst_t *p_bulk_info = nullptr);
+    bool doBulkRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, uint8_t *p_err_list, uint8_t err_list_size, InfoSyncBulkInst_t *p_bulk_info = nullptr);
 
     /* Easy functions for Bulk Write */
-    bool beginBulkWrite();
-    bool addBulkWriteData(uint8_t id, uint16_t addr, uint8_t *p_data, uint16_t data_len);
-    bool sendBulkWrite();
+    bool beginSetupBulkWrite(InfoSyncBulkInst_t *p_bulk_info = nullptr);
+    bool addBulkWriteData(uint8_t id, uint16_t addr, uint8_t *p_data, uint16_t data_len, InfoSyncBulkInst_t *p_bulk_info = nullptr);
+    bool endSetupBulkWrite(InfoSyncBulkInst_t *p_bulk_info = nullptr);
+    bool doBulkWrite(InfoSyncBulkInst_t *p_bulk_info = nullptr);
 #endif    
 
     uint8_t getLastStatusPacketError() const;