int DynamixelSimpleAPI::getSetting(const int id, const int reg_name, int reg_type, const int device)
{
int value = -1;
if (checkId(id) == true)
{
// Device detection
const int (*cctt)[8] = getRegisterTable(device);
if (cctt == NULL)
{
// Using default control table from this SimpleAPI isntance
cctt = ct;
}
if (cctt)
{
// Find register's informations (addr, size...)
RegisterInfos infos = {-1, -1, -1, -1, -1, -1, -1, -1, -1};
if (getRegisterInfos(cctt, reg_name, infos) == 1)
{
// Read value
if (infos.reg_size == 1)
{
value = dxl_read_byte(id, infos.reg_addr);
}
else if (infos.reg_size == 2)
{
value = dxl_read_word(id, infos.reg_addr);
}
// Check value
if (value < infos.reg_value_min && value > infos.reg_value_max)
{
value = -1;
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] getSetting(reg %i / %s) [REGISTER NAME ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] getSetting(reg %i / %s) [CONTROL TABLE ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] getSetting(reg %i / %s) [DEVICE ID ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
return value;
}
ServoDRS::ServoDRS(int herkulex_id, int herkulex_model, int control_mode):
ServoHerkuleX(getRegisterTable(hkx_get_servo_model(herkulex_model)), herkulex_id, herkulex_model, control_mode)
{
int servo_model = hkx_get_servo_model(herkulex_model);
if (servo_model == SERVO_DRS_0402 || servo_model == SERVO_DRS_0602)
{
runningDegrees = 360;
steps = 12962;
}
else if (servo_model == SERVO_DRS_0401 || servo_model == SERVO_DRS_0601)
{
runningDegrees = 320;
steps = 2048;
}
else //if (servo_model == SERVO_DRS_0101 || servo_model == SERVO_DRS_0201)
{
runningDegrees = 320;
steps = 1024;
}
}
int DynamixelSimpleAPI::setSetting(const int id, const int reg_name, const int reg_value, int reg_type, const int device)
{
int status = 0;
if (checkId(id) == true)
{
// Device detection
const int (*cctt)[8] = getRegisterTable(device);
if (cctt == NULL)
{
// Using default control table from this SimpleAPI isntance
cctt = ct;
}
if (cctt)
{
// Find register's informations (addr, size...)
RegisterInfos infos = {-1, -1, -1, -1, -1, -1, -1, -1, -1};
if (getRegisterInfos(cctt, reg_name, infos) == 1)
{
// Check if we have permission to write into this register
if (infos.reg_access_mode == READ_WRITE)
{
// Check value
if (reg_value >= infos.reg_value_min && reg_value <= infos.reg_value_max)
{
// Write value
if (infos.reg_size == 1)
{
dxl_write_byte(id, infos.reg_addr, reg_value);
}
else if (infos.reg_size == 2)
{
dxl_write_word(id, infos.reg_addr, reg_value);
}
// Check for error
if (dxl_print_error() == 0)
{
status = 1;
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [VALUE ERROR] (min: %i / max: %i)\n",
id, reg_name, getRegisterNameTxt(reg_name).c_str(), infos.reg_value_min, infos.reg_value_max);
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [REGISTER ACCESS ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [REGISTER NAME ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [CONTROL TABLE ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] setSetting(reg %i / %s) [DEVICE ID ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
return status;
}
