bool SubSet::checkPath(Element *elem)
{
if (checkId(elem)) {
return true;
}
for (Iterator it = childrenbegin(elem); !it.end(); ++it) {
if (checkId(it.getElement())) {
return true;
}
if (checkPath(it.getElement())) {
return true;
}
}
return false;
}
void DynamixelSimpleAPI::reboot(const int id)
{
if (checkId(id) == true)
{
dxl_reboot(id);
}
}
void DynamixelSimpleAPI::reset(const int id, const int setting)
{
if (checkId(id) == true)
{
dxl_reset(id, setting);
}
}
int DynamixelSimpleAPI::readCurrentPosition(const int id)
{
int value = -1;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_CURRENT_POSITION);
value = dxl_read_word(id, addr);
if (dxl_print_error() == 0)
{
// Valid positions are in range [0:1023] for most servo series, and [0:4095] for high-end servo series
if ((value < 0) || (value > 4095))
{
value = -1;
}
}
else
{
value = -1;
}
}
return value;
}
int DynamixelSimpleAPI::readCurrentLoad(const int id)
{
int value = -1;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_CURRENT_LOAD);
value = dxl_read_word(id, addr);
if (dxl_print_error() == 0)
{
// Valid loads are in range [0:2047]
if ((value < 0) || (value > 2047))
{
value = -1;
}
}
else
{
value = -1;
}
}
return value;
}
void DynamixelSimpleAPI::action(const int id)
{
if (checkId(id) == true)
{
dxl_action(id);
}
}
int DynamixelSimpleAPI::readCurrentSpeed(const int id)
{
int value = -1;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_CURRENT_SPEED);
value = dxl_read_word(id, addr);
if (dxl_print_error() == 0)
{
// Valid speeds are in range [0:1023] for "Join Mode", [0:2047] for "Wheel Mode"
if ((value < 0) || (value > 2047))
{
value = -1;
}
}
else
{
value = -1;
}
}
return value;
}
int DynamixelSimpleAPI::getMaxTorque(const int id)
{
int value = -1;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_MAX_TORQUE);
value = dxl_read_word(id, addr);
if (dxl_print_error() == 0)
{
// Valid torques are in range [0:1023]
if ((value < 0) || (value > 1023))
{
value = -1;
}
}
else
{
value = -1;
}
}
return value;
}
int DynamixelSimpleAPI::setGoalPosition(const int id, const int position, const int speed)
{
int status = 0;
if (checkId(id) == true)
{
// Set goal speed, and if success proceed to set goal position
if (setGoalSpeed(id, speed) != -1)
{
// Valid positions are in range [0:1023] for most servo series, and [0:4095] for high-end servo series
if ((position >= 0) && (position <= 4095))
{
int addr = getRegisterAddr(ct, REG_GOAL_POSITION);
dxl_write_word(id, addr, position);
if (dxl_print_error() == 0)
{
status = 1;
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] Cannot set goal position '%i' for this servo: out of range\n", id, position);
}
}
}
return status;
}
int DynamixelSimpleAPI::setLed(const int id, int value, const int color)
{
int status = 0;
if (checkId(id) == true)
{
// Normalize value
if (value >= 1)
{
(servoSerie == SERVO_XL) ? value = color : value = 1;
}
else
{
value = 0;
}
// Execute command
int addr = getRegisterAddr(ct, REG_LED);
if (addr >= 0)
{
dxl_write_byte(id, addr, value);
// Check for error
if (dxl_print_error() == 0)
{
status = 1;
}
}
}
return status;
}
int DynamixelSimpleAPI::setMinMaxPositions(const int id, const int min, const int max)
{
int status = 0;
if (checkId(id) == true)
{
// Valid positions are in range [0:1023] for most servo series, and [0:4095] for high-end servo series
if ((min < 0) || (min > 4095) || (max < 0) || (max > 4095))
{
TRACE_ERROR(DAPI, "[#%i] Cannot set new min/max positions '%i/%i' for this servo: out of range\n", id, min, max);
}
else
{
int addr_min = getRegisterAddr(ct, REG_MIN_POSITION);
int addr_max = getRegisterAddr(ct, REG_MAX_POSITION);
// Write min value
dxl_write_word(id, addr_min, min);
if (dxl_print_error() == 0)
{
status = 1;
}
// Write max value
dxl_write_word(id, addr_max, max);
if (dxl_print_error() == 0)
{
status = 1;
}
}
}
return status;
}
int DynamixelSimpleAPI::changeBaudRate(const int id, const int baudnum)
{
int status = 0;
if (checkId(id) == true)
{
// Valid baudnums are in range [0:254]
if ((baudnum >= 0) && (baudnum <= 254))
{
int addr = getRegisterAddr(ct, REG_BAUD_RATE);
dxl_write_byte(id, addr, baudnum);
if (dxl_print_error() == 0)
{
status = 1;
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] Cannot set new baudnum '%i' for this servo: out of range\n", id, baudnum);
}
}
return status;
}
/*
* 获取命令
*/
void getCommand(void)
{
while (uartRecvData == '\0');
//printf ("0x%x", uartRecvData);
switch (uartRecvData)
{
//检测NRF模块是否正常
case 'c':
checkId();
break;
//扫描可用的终端地址
case 's':
scanAddr();
//checkId();
break;
//测试命令
case 't':
printf ("recv a command\n");
break;
case 'j':
mode = NOMAL_MODE;
break;
default :
printf ("ERROR COMMAND: 0x%x\n", uartRecvData);
}
//清空数据接收缓存
uartRecvData = '\0';
}
void joystick_map_button(int id, int butnum, char key)
{
checkId(void());
if ((unsigned)id >= enigma::joysticks.size())
return;
enigma::e_joystick *js = enigma::joysticks[id];
if (js and butnum < js->buttoncount)
js->wrapbutton[butnum] = key;
}
int joystick_direction(int id)
{
checkId(0);
enigma::e_joystick * const js = enigma::joysticks[id];
if (js->axiscount < 2) return 0;
const int x = js->axis[0] < -.5 ? 0 : js->axis[0] > .5 ? 2 : 1;
const int y = js->axis[1] < -.5 ? 0 : js->axis[1] > .5 ? 6 : 3;
return 97 + x + y;
}
void joystick_map_axis(int id, int axisnum, char keyneg, char keypos)
{
checkId(void());
if ((unsigned)id >= enigma::joysticks.size())
return;
enigma::e_joystick *js = enigma::joysticks[id];
if (js and axisnum < js->axiscount)
js->wrapaxis_negative[axisnum] = keyneg,
js->wrapaxis_positive[axisnum] = keypos;
}
int DynamixelSimpleAPI::turn(const int id, const int velocity)
{
int status = 0;
if (checkId(id) == true)
{
// TODO
}
return status;
}
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;
}
IBase * EntityMgr::queryInterface(entid_t eid, iid_t iid, bool softQuery) {
IBase * pInt = NULL;
if (eid>=0) {
checkId(eid);
pInt = chunkEntity[eid].entity->entityAskInterface(iid);
}
if (NULL==pInt && !softQuery) {
const char * iname = getCore()->getIdName(iid);
throw Exception("core: Entity {eid=%d} doesn't support Iterface{iid=0x%08X, name='%s'}", eid, iid, iname);
}
return (IBase*)vtableDisplace.displaceInterface(pInt);
}
void EntityMgr::handleTrigger(entid_t id, trigid_t trigId, Object * param, bool immediate) {
checkId(id);
if (immediate) {
const EntityChunk& ec=chunkEntity[id];
ec.entity->entityHandleEvent(EvtTrigger::eventId,trigId,param);
}
else {
EntityChunk& ec=chunkEntity[id];
ec.trigevt.push_back(EntityTriggerEvent());
ec.trigevt.back().id = trigId;
ec.trigevt.back().param = param;
}
}
int DynamixelSimpleAPI::readFirmwareVersion(const int id)
{
int value = -1;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_FIRMWARE_VERSION);
value = dxl_read_byte(id, addr);
if (dxl_print_error() != 0)
{
value = -1;
}
}
return value;
}
int DynamixelSimpleAPI::readModelNumber(const int id)
{
int value = -1;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_MODEL_NUMBER);
value = dxl_read_word(id, addr);
if (dxl_print_error() != 0)
{
value = -1;
}
}
return value;
}
int DynamixelSimpleAPI::getLed(const int id)
{
int value = -1;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_LED);
value = dxl_read_byte(id, addr);
if (dxl_print_error() != 0)
{
value = -1;
}
}
return value;
}
bool DynamixelSimpleAPI::isMoving(const int id)
{
bool moving = false;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_MOVING);
if (dxl_read_byte(id, addr) > 0)
{
if (dxl_print_error() == 0)
{
moving = true;
}
}
}
return moving;
}
int ButtonGroup::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QWidget::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: buttonClicked((*reinterpret_cast< int(*)>(_a[1]))); break;
case 1: buttonReleased((*reinterpret_cast< int(*)>(_a[1]))); break;
case 2: buttonPressed((*reinterpret_cast< int(*)>(_a[1]))); break;
default: ;
}
_id -= 3;
}
#ifndef QT_NO_PROPERTIES
else if (_c == QMetaObject::ReadProperty) {
void *_v = _a[0];
switch (_id) {
case 0: *reinterpret_cast< int*>(_v) = getCheckedId(); break;
}
_id -= 1;
} else if (_c == QMetaObject::WriteProperty) {
void *_v = _a[0];
switch (_id) {
case 0: checkId(*reinterpret_cast< int*>(_v)); break;
}
_id -= 1;
} else if (_c == QMetaObject::ResetProperty) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyDesignable) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyScriptable) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyStored) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyEditable) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyUser) {
_id -= 1;
}
#endif // QT_NO_PROPERTIES
return _id;
}
int DynamixelSimpleAPI::setMaxTorque(const int id, const int torque)
{
int status = 0;
if (checkId(id, false) == true)
{
// Valid torques are in range [0:1023]
if ((torque >= 0) && (torque <= 1023))
{
int addr = getRegisterAddr(ct, REG_MAX_TORQUE);
dxl_write_word(id, addr, torque);
if (dxl_print_error() == 0)
{
status = 1;
}
}
}
return status;
}
int ButtonGroup::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QWidget::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
if (_id < 3)
qt_static_metacall(this, _c, _id, _a);
_id -= 3;
}
#ifndef QT_NO_PROPERTIES
else if (_c == QMetaObject::ReadProperty) {
void *_v = _a[0];
switch (_id) {
case 0: *reinterpret_cast< bool*>(_v) = getCheckedId(); break;
}
_id -= 1;
} else if (_c == QMetaObject::WriteProperty) {
void *_v = _a[0];
switch (_id) {
case 0: checkId(*reinterpret_cast< bool*>(_v)); break;
}
_id -= 1;
} else if (_c == QMetaObject::ResetProperty) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyDesignable) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyScriptable) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyStored) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyEditable) {
_id -= 1;
} else if (_c == QMetaObject::QueryPropertyUser) {
_id -= 1;
}
#endif // QT_NO_PROPERTIES
return _id;
}
double DynamixelSimpleAPI::readCurrentVoltage(const int id)
{
double value = 0.0;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_CURRENT_VOLTAGE);
value = dxl_read_byte(id, addr);
// Valid voltages are in range [9;12], but there is not much point in validating this value
if (dxl_print_error() == 0)
{
value /= 10.0;
}
else
{
value = 0.0;
}
}
return value;
}
double DynamixelSimpleAPI::readCurrentTemperature(const int id)
{
double value = 0.0;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_CURRENT_TEMPERATURE);
value = dxl_read_byte(id, addr);
// Valid temperatures are in range [-5;+70], but there is not much point in validating this value
if (dxl_print_error() == 0)
{
value /= 10.0;
}
else
{
value = 0.0;
}
}
return value;
}
int DynamixelSimpleAPI::setTorqueEnabled(const int id, int torque)
{
int status = 0;
if (checkId(id) == true)
{
int addr = getRegisterAddr(ct, REG_TORQUE_ENABLE);
// Valid torque are in range [0:1]
if (torque != 0)
{
torque = 1;
}
dxl_write_byte(id, addr, torque);
if (dxl_print_error() == 0)
{
status = 1;
}
}
return status;
}
