void HerkuleX::hkx_reset(const int id, int setting, const int ack)
{
while(commLock);
txPacket[PKT_LENGTH] = 9;
txPacket[PKT_ID] = get_lowbyte(id);
txPacket[PKT_CMD] = CMD_ROLLBACK;
if (setting == RESET_ALL_EXCEPT_ID)
{
txPacket[PKT_DATA] = 1;
txPacket[PKT_DATA+1] = 0;
}
else if (setting == RESET_ALL_EXCEPT_ID_BAUDRATE)
{
txPacket[PKT_DATA] = 1;
txPacket[PKT_DATA+1] = 1;
}
else
{
// RESET_ALL
}
hkx_txrx_packet(ack);
}
bool HerkuleX::hkx_ping(const int id, PingResponse *status, const int ack)
{
bool retcode = false;
while(commLock);
// We do not use a READ instruction directly instead of STAT, because it may
// not receive an answer depending on ack policy value.
txPacket[PKT_ID] = get_lowbyte(id);
txPacket[PKT_CMD] = CMD_STAT;
txPacket[PKT_LENGTH] = 7;
hkx_txrx_packet(ack);
if (commStatus == COMM_RXSUCCESS)
{
retcode = true;
if (status != NULL)
{
// Emulate ping response from Dynamixel protocol v2
status->model_number = hkx_read_word(id, 0, REGISTER_ROM, ack);
status->firmware_version = hkx_read_word(id, 2, REGISTER_ROM, ack);
}
}
return retcode;
}
void HerkuleX::hkx_reboot(const int id, const int ack)
{
while(commLock);
txPacket[PKT_LENGTH] = 7;
txPacket[PKT_ID] = get_lowbyte(id);
txPacket[PKT_CMD] = CMD_REBOOT;
hkx_txrx_packet(ack);
}
void HerkuleX::hkx_write_byte(const int id, const int address, const int value, const int register_type, const int ack)
{
while(commLock);
txPacket[PKT_LENGTH] = 7 + 3;
txPacket[PKT_ID] = get_lowbyte(id);
if (register_type == REGISTER_RAM)
txPacket[PKT_CMD] = CMD_RAM_WRITE;
else
txPacket[PKT_CMD] = CMD_EEP_WRITE;
txPacket[PKT_DATA] = get_lowbyte(address);
txPacket[PKT_DATA+1] = 1;
txPacket[PKT_DATA+2] = get_lowbyte(value);
hkx_txrx_packet(ack);
}
int HerkuleX::hkx_read_word(const int id, const int address, const int register_type, const int ack)
{
int value = -1;
if (id == 254)
{
TRACE_ERROR(HKX, "Cannot send 'Read' instruction to broadcast address!\n");
}
else if (ack == ACK_NO_REPLY)
{
TRACE_ERROR(HKX, "Cannot send 'Read' instruction if ACK_NO_REPLY is set!\n");
}
else
{
while(commLock);
txPacket[PKT_LENGTH] = 7 + 2;
txPacket[PKT_ID] = get_lowbyte(id);
if (register_type == REGISTER_RAM)
txPacket[PKT_CMD] = CMD_RAM_READ;
else
txPacket[PKT_CMD] = CMD_EEP_READ;
txPacket[PKT_DATA] = get_lowbyte(address);
txPacket[PKT_DATA+1] = 2;
hkx_txrx_packet(ack);
if ((ack == ACK_DEFAULT && ackPolicy > ACK_NO_REPLY) ||
(ack > ACK_NO_REPLY))
{
if (commStatus == COMM_RXSUCCESS)
{
value = make_short_word(rxPacket[PKT_DATA+2], rxPacket[PKT_DATA+3]);
}
else
{
value = commStatus;
}
}
}
return value;
}
void HerkuleX::hkx_s_jog(const int id, const int mode, const int value, const int ack)
{
int JOG = 0;
int SET = 0;
while(commLock);
txPacket[PKT_LENGTH] = 7 + 5;
txPacket[PKT_ID] = get_lowbyte(id);
txPacket[PKT_CMD] = CMD_S_JOG;
txPacket[PKT_DATA] = 0x3c; // playtime
if (mode == 0) // Position control
{
JOG = value; // goal position
SET = 0x04; // position control with green led
}
else // if (mode == 1) // Continuous rotation
{
if (value >= 0)
{
JOG = value; // goal position
}
else
{
JOG = std::abs(value); // speed
JOG += 0x4000; // direction
}
SET = 0x0A; // continuous rotation with blue led
}
// S_JOG(0)
txPacket[PKT_DATA+1] = get_lowbyte(JOG);
txPacket[PKT_DATA+2] = get_highbyte(JOG);
txPacket[PKT_DATA+3] = get_lowbyte(SET);
txPacket[PKT_DATA+4] = get_lowbyte(id); // id
hkx_txrx_packet(ack);
}
