dynamixel_tool.cpp

Go to the documentation of this file.

/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
 
/* Authors: Taehun Lim (Darby), Ryan Shim */
 
#include "../../include/dynamixel_workbench_toolbox/dynamixel_tool.h"
 
//===================================================================
// Define Serial ID to Namd table
//===================================================================
typedef struct 
{
  uint16_t      number;
  const char*   name; 
} DynamixelModel;
 
static const DynamixelModel dynamixel_model_table[] = {
    {AX_12A, "AX-12A"},
    {AX_12W, "AX-12W"},
    {AX_18A, "AX-18A"},
 
    {RX_10, "RX-10"},
    {RX_24F, "RX-24F"},
    {RX_28, "RX-28"},
    {RX_64, "RX-64"},
 
    {EX_106, "EX-106"},
 
    {MX_12W, "MX-12W"},
    {MX_28, "MX-28"},
    {MX_28_2, "MX-28-2"},
    {MX_64, "MX-64"},
    {MX_64_2, "MX-64-2"},
    {MX_106, "MX-106"},
    {MX_106_2, "MX-106-2"},
 
    {XL_320, "XL-320"},
    {XL330_M077, "XL330_M077"},
    {XL330_M288, "XL330_M288"},
    {XL430_W250, "XL430-W250"},
 
    {XL430_W250_2, "XL430-W250-2"}, // 2XL
    {XC430_W250_2, "XC430-W250-2"}, // 2XC
 
    {XC430_W150, "XC430-W150"},
    {XC430_W240, "XC430-W240"},
 
    {XM430_W210, "XM430-W210"},
    {XM430_W350, "XM430-W350"},
    
    {XM540_W150, "XM540-W150"},
    {XM540_W270, "XM540-W270"},
 
    {XH430_V210, "XH430-V210"},
    {XH430_V350, "XH430-V350"},
    {XH430_W210, "XH430-W210"},
    {XH430_W350, "XH430-W350"},
 
    {XH540_W150, "XH540_W150"},
    {XH540_W270, "XH540_W270"},
    {XH540_V150, "XH540_V150"},
    {XH540_V270, "XH540_V270"},
 
    {XW540_T260, "XW540_T260"},
    {XW540_T140, "XW540_T140"},
 
    {PRO_L42_10_S300_R, "PRO-L42-10-S300-R"},
    {PRO_L54_30_S400_R, "PRO-L54-30-S400-R"},
    {PRO_L54_30_S500_R, "PRO-L54-30-S500-R"},
    {PRO_L54_50_S290_R, "PRO-L54-50-S290-R"},
    {PRO_L54_50_S500_R, "PRO-L54-50-S500-R"},
 
    {PRO_M42_10_S260_R, "PRO-M42-10-S260-R"},
    {PRO_M54_40_S250_R, "PRO-M54-40-S250-R"},
    {PRO_M54_60_S250_R, "PRO-M54-60-S250-R"},
 
    {PRO_H42_20_S300_R,  "PRO-H42-20-S300-R"},
    {PRO_H54_100_S500_R, "PRO-H54-100-S500-R"},
    {PRO_H54_200_S500_R, "PRO-H54-200-S500-R"},
 
    {PRO_M42_10_S260_R_A, "PRO-M42-10-S260-R-A"},
    {PRO_M54_40_S250_R_A, "PRO-M54-40-S250-R-A"},
    {PRO_M54_60_S250_R_A, "PRO-M54-60-S250-R-A"},
 
    {PRO_H42_20_S300_R_A,  "PRO-H42-20-S300-R-A"},
    {PRO_H54_100_S500_R_A, "PRO-H54-100-S500-R-A"},
    {PRO_H54_200_S500_R_A, "PRO-H54-200-S500-R-A"},
 
    {PRO_PLUS_M42P_010_S260_R, "PRO-PLUS-M42P-010-S260-R"},
    {PRO_PLUS_M54P_040_S250_R, "PRO-PLUS-M54P-040-S250-R"},
    {PRO_PLUS_M54P_060_S250_R, "PRO-PLUS-M54P-060-S250-R"},
 
    {PRO_PLUS_H42P_020_S300_R, "PRO-PLUS-H42P-020-S300-R"},
    {PRO_PLUS_H54P_100_S500_R, "PRO-PLUS-H54P-100-S500-R"},
    {PRO_PLUS_H54P_200_S500_R, "PRO-PLUS-H54P-200-S500-R"},
 
    {RH_P12_RN, "RH-P12-RN"},
 
    {RH_P12_RN_A, "RH-P12-RN-A"}
};
#define COUNT_DYNAMIXEL_MODEL  (sizeof(dynamixel_model_table)/sizeof(dynamixel_model_table[0]))
 
DynamixelTool::DynamixelTool() : dxl_cnt_(0), the_number_of_control_item_(0){}
 
DynamixelTool::~DynamixelTool(){}
 
void DynamixelTool::initTool(void)
{
  for (uint8_t i = 0; i < DYNAMIXEL_BUFFER; i++)
    dxl_id_[i] = 0;
 
  dxl_cnt_ = 0;
}
 
bool DynamixelTool::addTool(const char *model_name, uint8_t id, const char **log)
{
  bool result = false;
  initTool();
 
  model_name_ = model_name;
  result = setModelNumber(model_name, log);
  if (result == false) return false;
  dxl_id_[dxl_cnt_++] = id;
 
  result = setControlTable(model_name, log);
  if (result == false) return false;
 
  return true;
}
 
bool DynamixelTool::addTool(uint16_t model_number, uint8_t id, const char **log)
{
  bool result = false;
  initTool();
 
  result = setModelName(model_number, log);
  if (result == false) return false;
  model_number_ = model_number;
  dxl_id_[dxl_cnt_++] = id;
 
  result = setControlTable(model_number, log);
  if (result == false) return false;
 
  return result;
}
 
void DynamixelTool::addDXL(uint8_t id)
{
  dxl_id_[dxl_cnt_++] = id;
}
 
bool DynamixelTool::setControlTable(const char *model_name, const char **log)
{  
  const char* name = model_name;
  uint8_t name_length = strlen(name);
 
  for (uint8_t index=0; index < COUNT_DYNAMIXEL_MODEL; index++)
  {
    if(strncmp(name, dynamixel_model_table[index].name, name_length) == 0)
    {
      return setControlTable(dynamixel_model_table[index].number, log);
    }
  }
 
  if (log != NULL)
    *log = "[DynamixelTool] Failed to set control table due to mismatch model name and model number";
  return false;
}
 
bool DynamixelTool::setControlTable(uint16_t model_number, const char **log)
{
  control_table_              = DynamixelItem::getControlTable(model_number);
  the_number_of_control_item_ = DynamixelItem::getTheNumberOfControlItem();
  model_info_                 = DynamixelItem::getModelInfo(model_number);
 
  if (control_table_ == NULL || model_info_ == NULL)
  {
    if (log != NULL)
      *log = "[DynamixelTool] Failed to get control table or model info";
    return false;
  }
 
  return true;
}
 
bool DynamixelTool::setModelName(uint16_t model_number, const char **log)
{
  uint16_t num = model_number;
 
  for (uint8_t index=0; index < COUNT_DYNAMIXEL_MODEL; index++)
  {
    if (num == dynamixel_model_table[index].number)
    {
      model_name_ = dynamixel_model_table[index].name;
      return true;
    }
  }
 
  if (log != NULL)
    *log = "[DynamixelTool] Failed to find model name";
  return false;
}
 
bool DynamixelTool::setModelNumber(const char *model_name, const char **log)
{
  const char* name = model_name;
  uint8_t name_length = strlen(name);
 
  for (uint8_t index=0; index < COUNT_DYNAMIXEL_MODEL; index++)
  {
    if(strncmp(name, model_name_, name_length) == 0)
    {
      model_number_ = dynamixel_model_table[index].number;
      return true;
    }
  }
 
  if (log != NULL)
    *log = "[DynamixelTool] Failed to find model number";
  return false;
}
 
const char* DynamixelTool::getModelName(void)
{
  return model_name_;
}
 
uint16_t DynamixelTool::getModelNumber(void)
{
  return model_number_;
}
 
const uint8_t* DynamixelTool::getID(void)
{
  const uint8_t* id_table_ = dxl_id_;
 
  return id_table_;
}
 
uint8_t DynamixelTool::getDynamixelCount(void)
{
  return dxl_cnt_;
}
 
uint8_t DynamixelTool::getDynamixelBuffer(void)
{
  return DYNAMIXEL_BUFFER;
}
 
float DynamixelTool::getRPM(void)
{
  return model_info_->rpm;
}
 
int64_t DynamixelTool::getValueOfMinRadianPosition(void)
{
  return model_info_->value_of_min_radian_position;
}
 
int64_t DynamixelTool::getValueOfMaxRadianPosition(void)
{
  return model_info_->value_of_max_radian_position;
}
 
int64_t DynamixelTool::getValueOfZeroRadianPosition(void)
{
  return model_info_->value_of_zero_radian_position;
}
 
float DynamixelTool::getMinRadian(void)
{
  return model_info_->min_radian;
}
 
float DynamixelTool::getMaxRadian(void)
{
  return model_info_->max_radian;
}
 
uint8_t DynamixelTool::getTheNumberOfControlItem(void)
{
  return the_number_of_control_item_;
}
 
const ControlItem *DynamixelTool::getControlItem(const char *item_name, const char **log)
{
  const ControlItem* control_item = control_table_;  
  uint8_t name_length = strlen(item_name);
 
  for (uint8_t num = 0; num < the_number_of_control_item_; num++)
  {
    if ((name_length == control_item->item_name_length) && 
        (memcmp(item_name, control_item->item_name, name_length) == 0))
    {
      return control_item;
    }
    control_item++;
  }
 
  if (log != NULL)
    *log = "[DynamixelTool] Can't find Item";
  return NULL;
}
 
const ControlItem *DynamixelTool::getControlTable(void)
{
  return control_table_;
}
 
const ModelInfo *DynamixelTool::getModelInfo(void)
{
  return model_info_;
}