void HerkuleX::hkx_txrx_packet(int ack)
{
#ifdef LATENCY_TIMER
// Latency timer for a complete transaction (instruction sent and status received)
std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
start = std::chrono::high_resolution_clock::now();
#endif
hkx_tx_packet();
if (commStatus != COMM_TXSUCCESS)
{
TRACE_ERROR(HKX, "Unable to send TX packet on serial link: '%s'\n", serialGetCurrentDevice().c_str());
return;
}
// Depending on 'ackPolicy' value and current instruction, we wait for an answer to the packet we just sent
if (ack == ACK_DEFAULT)
{
ack = ackPolicy;
}
if (ack != ACK_NO_REPLY)
{
int cmd = txPacket[PKT_CMD];
if ((ack == ACK_REPLY_ALL) ||
(ack == ACK_REPLY_READ && (cmd == CMD_STAT || cmd == CMD_EEP_READ || cmd == CMD_RAM_READ)))
{
do {
hkx_rx_packet();
}
while (commStatus == COMM_RXWAITING);
}
else
{
commStatus = COMM_RXSUCCESS;
commLock = 0;
}
}
else
{
commStatus = COMM_RXSUCCESS;
commLock = 0;
}
#ifdef PACKET_DEBUGGER
printTxPacket();
printRxPacket();
#endif
#ifdef LATENCY_TIMER
end = std::chrono::high_resolution_clock::now();
int loopd = std::chrono::duration_cast<std::chrono::microseconds>(end-start).count();
TRACE_1(HKX, "TX > RX loop: %iµs\n", loopd);
#endif
}
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;
}
