std::vector <int> DynamixelSimpleAPI::servoScan(int start, int stop)
{
// Check start/stop boundaries
if (start < 0 || start > (maxId - 1))
start = 0;
if (stop < 1 || stop > maxId || stop < start)
stop = maxId;
TRACE_INFO(DAPI, "> Scanning for Dynamixel devices on '%s'... Range is [%i,%i]\n",
serialGetCurrentDevice().c_str(), start, stop);
// A vector of Dynamixel IDs found during the scan
std::vector <int> ids;
for (int id = start; id <= stop; id++)
{
PingResponse pingstats;
// If the ping gets a response, then we have found a servo
if (dxl_ping(id, &pingstats) == true)
{
setLed(id, 1, LED_GREEN);
ids.push_back(id);
TRACE_INFO(DAPI, "[#%i] Dynamixel servo found!\n", id);
TRACE_INFO(DAPI, "[#%i] model: '%i' (%s)\n", id, pingstats.model_number,
dxl_get_model_name(pingstats.model_number).c_str());
// Other informations, not printed by default:
TRACE_1(DAPI, "[#%i] firmware: '%i' \n", id, pingstats.firmware_version);
TRACE_1(DAPI, "[#%i] position: '%i' \n", id, readCurrentPosition(id));
TRACE_1(DAPI, "[#%i] speed: '%i' \n", id, readCurrentSpeed(id));
TRACE_1(DAPI, "[#%i] torque: '%i' \n", id, getTorqueEnabled(id));
TRACE_1(DAPI, "[#%i] load: '%i' \n", id, readCurrentLoad(id));
TRACE_1(DAPI, "[#%i] baudrate: '%i' \n", id, getSetting(id, REG_BAUD_RATE));
setLed(id, 0);
}
else
{
printf(".");
}
}
printf("\n");
return ids;
}
std::string ServoDynamixel::getModelString()
{
std::lock_guard <std::mutex> lock(access);
return dxl_get_model_name(registerTableValues[gid(REG_MODEL_NUMBER)]);
}
