Merge pull request #70 from ROBOTIS-GIT/develop

Develop

.gitignore

@@ -1,2 +1,3 @@
 /.project
 /.vscode
+.DS_Store

library.properties

@@ -1,5 +1,5 @@
 name=Dynamixel2Arduino
-version=0.4.7
+version=0.4.8
 author=ROBOTIS
 license=Apache-2.0
 maintainer=Will Son([email protected])

src/Dynamixel2Arduino.cpp

@@ -33,6 +33,10 @@ const uint16_t model_number_table[] PROGMEM = {
     XL320,
     XL330_M288,
     XL330_M077,
+    XC330_M181,
+    XC330_M288,    
+    XC330_T181,
+    XC330_T288,    
     XL430_W250,
     XXL430_W250,
     XC430_W150,  XC430_W240,
@@ -298,7 +302,10 @@ bool Dynamixel2Arduino::setBaudrate(uint8_t id, uint32_t baudrate)
           return false;                    
       }    
       break;
-
+    case XC330_M288:
+    case XC330_M181:
+    case XC330_T288:
+    case XC330_T181:
     case XL330_M288:
     case XL330_M077:
       switch(baudrate)
@@ -652,6 +659,10 @@ bool Dynamixel2Arduino::setOperatingMode(uint8_t id, uint8_t mode)
     case MX106_2:
     case XL330_M288:
     case XL330_M077:
+    case XC330_M288:
+    case XC330_M181:    
+    case XC330_T181:
+    case XC330_T288:    
     case XM430_W210:
     case XM430_W350:
     case XH430_V210:
@@ -1088,6 +1099,7 @@ const ModelDependencyFuncItemAndRangeInfo_t dependency_ctable_2_0_common[] PROGM
   || ENABLE_ACTUATOR_MX64_PROTOCOL2 \
   || ENABLE_ACTUATOR_MX106_PROTOCOL2 \
   || ENABLE_ACTUATOR_XL330 \
+  || ENABLE_ACTUATOR_XC330 \
   || ENABLE_ACTUATOR_XC430 \
   || ENABLE_ACTUATOR_XL430 \
   || ENABLE_ACTUATOR_XM430 || ENABLE_ACTUATOR_XH430 \
@@ -1131,6 +1143,29 @@ const ModelDependencyFuncItemAndRangeInfo_t dependency_xl330_M288_M077[] PROGMEM
   {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
 };
 
+const ModelDependencyFuncItemAndRangeInfo_t dependency_xc330_m181_m288[] PROGMEM = {
+#if (ENABLE_ACTUATOR_XC330)
+  {SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -2352, 2352, 1},
+  {GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -2352, 2352, 1},
+
+  {SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -2047, 2047, 0.229},
+  {GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -2047, 2047, 0.229},
+#endif
+  {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
+};
+
+const ModelDependencyFuncItemAndRangeInfo_t dependency_xc330_t181_t288[] PROGMEM = {
+#if (ENABLE_ACTUATOR_XC330)
+  {SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -910, 910, 1},
+  {GET_CURRENT, PRESENT_CURRENT, UNIT_MILLI_AMPERE, -910, 910, 1},
+
+  {SET_VELOCITY, GOAL_VELOCITY, UNIT_RPM, -2047, 2047, 0.229},
+  {GET_VELOCITY, PRESENT_VELOCITY, UNIT_RPM, -2047, 2047, 0.229},
+#endif
+  {LAST_DUMMY_FUNC, ControlTableItem::LAST_DUMMY_ITEM, UNIT_RAW, 0, 0, 0}
+};
+
+
 const ModelDependencyFuncItemAndRangeInfo_t dependency_xm430_w210_w350[] PROGMEM = {
 #if (ENABLE_ACTUATOR_XM430)
   {SET_CURRENT, GOAL_CURRENT, UNIT_MILLI_AMPERE, -1193, 1193, 2.69},
@@ -1426,6 +1461,18 @@ static ItemAndRangeInfo_t getModelDependencyFuncInfo(uint16_t model_num, uint8_t
       p_dep_ctable = dependency_xl330_M288_M077;
       break;
 
+    case XC330_M181:
+    case XC330_M288:
+      p_common_ctable = dependency_ctable_2_0_common;
+      p_dep_ctable = dependency_xc330_m181_m288;
+      break;
+
+    case XC330_T181:
+    case XC330_T288:
+      p_common_ctable = dependency_ctable_2_0_common;
+      p_dep_ctable = dependency_xc330_t181_t288;
+      break;
+
     case XM430_W210:
     case XM430_W350:
       p_common_ctable = dependency_ctable_2_0_common;

src/actuator.cpp

@@ -169,6 +169,7 @@ const ModelControlTableInfo_t control_table_2_0[] PROGMEM = {
  || ENABLE_ACTUATOR_MX64_PROTOCOL2 \
  || ENABLE_ACTUATOR_MX106_PROTOCOL2 \
  || ENABLE_ACTUATOR_XL330 \
+ || ENABLE_ACTUATOR_XC330 \
  || ENABLE_ACTUATOR_XL430 \
  || ENABLE_ACTUATOR_XC430 \
  || ENABLE_ACTUATOR_XM430 || ENABLE_ACTUATOR_XH430 \
@@ -222,7 +223,6 @@ const ModelControlTableInfo_t control_table_2_0[] PROGMEM = {
   {ControlTableItem::VELOCITY_TRAJECTORY,    136, 4},
   {ControlTableItem::POSITION_TRAJECTORY,    140, 4},  
   {ControlTableItem::PRESENT_INPUT_VOLTAGE,  144, 2},
-  {ControlTableItem::PRESENT_VOLTAGE,        144, 2},
   {ControlTableItem::PRESENT_TEMPERATURE,    146, 1},
 #endif
   {ControlTableItem::LAST_DUMMY_ITEM,          0, 0}
@@ -257,6 +257,7 @@ const ModelControlTableInfo_t xc430_xl430_control_table[] PROGMEM = {
 
 const ModelControlTableInfo_t xmh430_xl330_control_table[] PROGMEM = {
 #if (ENABLE_ACTUATOR_XL330 \
+ || ENABLE_ACTUATOR_XC330 \
  || ENABLE_ACTUATOR_XM430 \
  || ENABLE_ACTUATOR_XH430)
   {ControlTableItem::CURRENT_LIMIT,           38, 2},
@@ -485,6 +486,10 @@ ControlTableItemInfo_t DYNAMIXEL::getControlTableItemInfo(uint16_t model_num, ui
 
     case XL330_M288:
     case XL330_M077:
+    case XC330_M181:
+    case XC330_M288:
+    case XC330_T181:
+    case XC330_T288:    
     case XM430_W210:
     case XM430_W350:
     case XH430_V210:

src/actuator.h

@@ -79,6 +79,20 @@
 #define XL330_M288          (uint16_t)1200
 #endif
 
+#ifndef XC330_M181
+#define XC330_M181          (uint16_t)1230
+#endif
+#ifndef XC330_M288
+#define XC330_M288          (uint16_t)1240
+#endif
+
+#ifndef XC330_T181
+#define XC330_T181          (uint16_t)1210
+#endif
+#ifndef XC330_T288
+#define XC330_T288          (uint16_t)1220
+#endif
+
 #ifndef XC430_W150
 #define XC430_W150          (uint16_t)1070
 #endif

src/utility/config.h

@@ -17,6 +17,7 @@
 
 #define ENABLE_ACTUATOR_XL320           1
 #define ENABLE_ACTUATOR_XL330           1
+#define ENABLE_ACTUATOR_XC330           1
 #define ENABLE_ACTUATOR_XL430           1   //Includes 2XL430
 #define ENABLE_ACTUATOR_XC430           1   //Includes 2XC430
 #define ENABLE_ACTUATOR_XM430           1

src/utility/slave.cpp

@@ -29,7 +29,7 @@ Slave::Slave(DXLPortHandler &port, const uint16_t model_num, float protocol_ver)
     packet_buf_capacity_ = DEFAULT_DXL_BUF_LENGTH;
     is_buf_malloced_ = true;
   }
-  info_tx_packet_.is_init = false;
+  info_tx_packet_.is_init = false;  
   info_rx_packet_.is_init = false;
 }
 
@@ -493,95 +493,131 @@ Slave::processInstWrite()
   p_rx_info = &info_rx_packet_;
   p_rx_param = p_rx_info->p_param_buf;
 
-  if(p_rx_info->id != DXL_BROADCAST_ID){
-    if(p_rx_info->protocol_ver == 2){
-      if(p_rx_info->recv_param_len > 2){ //2 = Address(2)+Data(n)
-        addr = ((uint16_t)p_rx_param[1]<<8) | (uint16_t)p_rx_param[0];
-        p_data = &p_rx_param[2];
-        data_length = p_rx_info->recv_param_len-2;
-        if(data_length+11 > packet_buf_capacity_){
-          err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
-        }
-      }else{
-        err = DXL_LIB_ERROR_WRONG_PACKET;
-      }
-    }else if(p_rx_info->protocol_ver == 1){
-      if(p_rx_info->recv_param_len > 1){ //1 = Address(1)+Data(n)
-        addr = p_rx_param[0];
-        p_data = &p_rx_param[1];
-        data_length = p_rx_info->recv_param_len-1;
-        if(data_length+6 > packet_buf_capacity_){
-          err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
-        }
-      }else{
-        err = DXL_LIB_ERROR_WRONG_PACKET;
+  if(p_rx_info->id == DXL_BROADCAST_ID){
+    last_lib_err_ = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST;
+    return false;
+  }
+
+  // extract start address and length from the instruction packet
+  switch (p_rx_info->protocol_ver)
+  {
+  case 2:
+    if(p_rx_info->recv_param_len <= 2) { //2 = Address(2)+Data(n)
+      err = DXL_LIB_ERROR_WRONG_PACKET;
+    } 
+    else {
+      addr = ((uint16_t)p_rx_param[1]<<8) | (uint16_t)p_rx_param[0];
+      p_data = &p_rx_param[2];
+      data_length = p_rx_info->recv_param_len-2;
+      if(data_length+11 > packet_buf_capacity_){
+        err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
       }
-    }else{
+    }
+    break;
+
+  case 1:  
+    if(p_rx_info->recv_param_len <= 1){ //1 = Address(1)+Data(n)
       err = DXL_LIB_ERROR_WRONG_PACKET;
     }
+    else {
+      addr = p_rx_param[0];
+      p_data = &p_rx_param[1];
+      data_length = p_rx_info->recv_param_len-1;
+      if(data_length+6 > packet_buf_capacity_){
+        err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
+      }
+    }
+    break;
 
-    if(err == DXL_LIB_OK){
-      uint8_t i, j, backup_data = 0;
-      uint16_t item_start_addr, item_addr_length;
-      ControlItem_t *p_item;
-      for(i=0; i < registered_item_cnt_; i++){
-        p_item = &control_table_[i];
-        item_start_addr = p_item->start_addr;
-        item_addr_length = p_item->length;
-        if(item_start_addr != ADDR_ITEM_MODEL_NUMBER
-        && item_start_addr != ADDR_ITEM_FIRMWARE_VER){
-          if(item_addr_length != 0
-          && p_item->p_data != nullptr
-          && isAddrInRange(item_start_addr, item_addr_length, addr, data_length) == true){
-            // Check data for ID, Protocol Version (Act as a system callback)
-            if(item_start_addr == ADDR_ITEM_ID){
-              backup_data = p_data[item_start_addr-addr];
-              if(protocol_ver_idx_ == 2 && backup_data >= 0xFD){
+  default:
+    err = DXL_LIB_ERROR_WRONG_PACKET;
+    break;
+  }
+
+  if(err == DXL_LIB_OK){
+    uint8_t i, j;
+    uint8_t backup_data[32];      // we max support registers of 32 bytes
+    uint16_t item_start_addr, item_addr_length;
+    ControlItem_t *p_item;
+
+    for(i=0; i < registered_item_cnt_; i++){
+
+      p_item = &control_table_[i];
+      item_start_addr = p_item->start_addr;
+      item_addr_length = p_item->length;
+
+      if(item_addr_length != 0 && p_item->p_data != nullptr
+      && isAddrInRange(item_start_addr, item_addr_length, addr, data_length) == true){
+
+        // backup data, copy new data
+        for(j=0; j<item_addr_length; j++){
+          // backup supported only for registers smaller than 32 bytes
+          if (item_addr_length <= 32) {
+            backup_data[j] = p_item->p_data[j];
+          }
+          p_item->p_data[j] = p_data[item_start_addr - addr + j];
+        }
+        // Check data for ID, Protocol Version (Act as a system callback)
+        switch (item_start_addr) {
+
+          case ADDR_ITEM_ID:      // validate ID
+            if(protocol_ver_idx_ == 2 && id_ >= 0xFD){
+              packet_err = DXL2_0_ERR_DATA_RANGE;
+            }
+            if(protocol_ver_idx_ == 1 && id_ >= 0xFE){
+              packet_err |= 1<<DXL1_0_ERR_RANGE_BIT;
+            }   
+            break;
+
+          case ADDR_ITEM_PROTOCOL_VER: // validate Protocol Version 
+            if(protocol_ver_idx_ != 1 && protocol_ver_idx_ != 2){
+              if(backup_data[0] == 2){
                 packet_err = DXL2_0_ERR_DATA_RANGE;
-              }else if(protocol_ver_idx_ == 1 && backup_data >= 0xFE){
-                packet_err |= 1<<DXL1_0_ERR_RANGE_BIT;
               }
-              backup_data = id_;             
-            }else if(item_start_addr == ADDR_ITEM_PROTOCOL_VER){  
-              backup_data = p_data[item_start_addr-addr];
-              if(backup_data != 1 && backup_data != 2){
-                if(protocol_ver_idx_ == 2){
-                  packet_err = DXL2_0_ERR_DATA_RANGE;
-                }else if(protocol_ver_idx_ == 1){
-                  packet_err |= 1<<DXL1_0_ERR_RANGE_BIT;
-                }                
+              else if(backup_data[0] == 1){
+                packet_err |= 1<<DXL1_0_ERR_RANGE_BIT;
               }
-              backup_data = protocol_ver_idx_;        
             }
-            if(packet_err != 0){
-              break;
+            break;
+
+          case ADDR_ITEM_MODEL_NUMBER:  // model number if Read Only
+          case ADDR_ITEM_FIRMWARE_VER:  // firmware is Read Only
+            if(backup_data[0] == 2){
+              packet_err = DXL2_0_ERR_DATA_RANGE;
             }
-            // Copy received data to each item
-            for(j=0; j<item_addr_length; j++){
-              p_item->p_data[j] = p_data[item_start_addr-addr+j];
+            else if(backup_data[0] == 1){
+              packet_err |= 1<<DXL1_0_ERR_RANGE_BIT;
             }
-            // Run user callback for Write instruction,
-            // then check and handle the returned error.
-            if(user_write_callback_ != nullptr){
-              user_write_callback_(item_start_addr, packet_err, user_write_callbakc_arg_);
-              if(packet_err != 0){
-                // If an error occurs for the ID and Protocol Version,
-                // restore the previous data. (Act as a system callback)
-                if(item_start_addr == ADDR_ITEM_ID){
-                  id_ = backup_data;
-                }else if(item_start_addr == ADDR_ITEM_PROTOCOL_VER){
-                  protocol_ver_idx_ = backup_data;
-                }   
-                break;
-              }
+            break;
+
+        }
+
+        // Run user callback for Write instruction
+        //
+        // NOTE: Slave does not implement EEPROM persistence of data.
+        // If your device needs to persist data to ROM you must implement
+        // `user_write_callback_` and there save the registers to ROM
+        // including the ones implemented in Slave (id, protocol, 
+        // model number, firmware version)
+        if (packet_err == 0 && user_write_callback_ != nullptr){
+          user_write_callback_(item_start_addr, packet_err, user_write_callbakc_arg_);
+        }
+
+        if(packet_err != 0){
+          // If an error occurs restore the previous data.
+          // backup supported only for registers smaller than 32 bytes
+          if (item_addr_length <= 32) {
+            for(j=0; j<item_addr_length; j++){
+              p_item->p_data[j] = backup_data[j];
             }
           }
         }
-      }
-      ret = txStatusPacket(id_, packet_err, nullptr, 0);
-    }
-  }else{
-    err = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST;
+
+      } // if
+
+    } // for
+
+    ret = txStatusPacket(id_, packet_err, nullptr, 0);
   }
 
   last_lib_err_ = err;
@@ -728,4 +764,4 @@ static bool isAddrInOtherItem(uint16_t start_addr, uint16_t length,
   }  
 
   return ret;
-}
+}