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_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);
}
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_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_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);
}
unsigned short HerkuleX::hkx_checksum_packet(unsigned char *packetData, const int packetSize)
{
// Generate checksum
unsigned short checksum = 0;
// (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n])&0xFE
int Sum1 = packetData[PKT_LENGTH];
Sum1 ^= packetData[PKT_ID];
Sum1 ^= packetData[PKT_CMD];
for (int i = PKT_DATA; i < packetSize; i++)
{
Sum1 ^= packetData[i];
}
Sum1 &= 0xFE;
//
int Sum2 = (~Sum1) & 0xFE;
// Make a word from these two bytes
checksum = get_lowbyte(Sum1);
checksum += get_lowbyte(Sum2) << 8;
return checksum;
}
int motor_spin(FT_HANDLE ftHandleDYNA, int motorNum, int speed)
{
unsigned char InstructionPacket[160] = {0};
// unsigned char StatusPacket[70]={0};
FT_STATUS ft_status;
//make the instruction packet
InstructionPacket[0] = 0xff;
InstructionPacket[1] = 0xff;
InstructionPacket[ID] = (unsigned char)motorNum;
InstructionPacket[LENGTH] = 5; //=parameter number +2
InstructionPacket[INSTRUCTION] = INST_WRITE;
InstructionPacket[PARAMETER] = (unsigned char) 32; //address for free spin
InstructionPacket[PARAMETER+1] = (unsigned char)get_lowbyte(speed);
InstructionPacket[PARAMETER+2] = (unsigned char)get_highbyte(speed);
unsigned char checksum = 0;
for(unsigned char i=0; i<(InstructionPacket[LENGTH]+1); i++ )
checksum += InstructionPacket[i+2];
InstructionPacket[InstructionPacket[LENGTH]+3] = ~checksum;
/* qDebug()<<"Packet:";
qDebug()<<InstructionPacket[0]<<" "<<InstructionPacket[1]<<" "
<<InstructionPacket[ID] <<" "<<InstructionPacket[LENGTH] <<" "
<<InstructionPacket[INSTRUCTION] <<" " <<InstructionPacket[PARAMETER]<<" "
<<InstructionPacket[PARAMETER+1] <<" "<< InstructionPacket[PARAMETER+2];
*/
//send the packet
unsigned char *pPacket = InstructionPacket;
int numPacket = 9; //length plus 4
DWORD dwNumToWrite = (DWORD)numPacket;
DWORD dwNumWritten;
ft_status = FT_Write(ftHandleDYNA, (LPVOID)pPacket, dwNumToWrite, &dwNumWritten );
if( ft_status == FT_IO_ERROR )
{
qDebug() << "\n\nError sending packet to motor!";
return 0;
}
if(numPacket != dwNumWritten)
{
qDebug() << "\n\nAll Bytes were not written to device!";
return 0;
}
return 1;
}
void HerkuleX::hkx_set_txpacket_parameter(int index, int value)
{
txPacket[PKT_DATA+index] = get_lowbyte(value);
}
void HerkuleX::hkx_set_txpacket_instruction(int instruction)
{
txPacket[PKT_CMD] = get_lowbyte(instruction);
}
void HerkuleX::hkx_set_txpacket_length_field(int length)
{
txPacket[PKT_LENGTH] = get_lowbyte(length);
}
void HerkuleX::hkx_set_txpacket_id(int id)
{
txPacket[PKT_ID] = get_lowbyte(id);
}
void HerkuleX::hkx_rx_packet()
{
if (serial == NULL)
{
TRACE_ERROR(HKX, "Serial interface is not initialized!\n");
return;
}
// No lock mean no packet has just been sent, so why wait for an answer (?)
if (commLock == 0)
{
return;
}
// Packet sent to a broadcast address? No need to wait for a status packet.
if (txPacket[PKT_ID] == BROADCAST_ID)
{
commStatus = COMM_RXSUCCESS;
commLock = 0;
return;
}
// Minimum status packet size estimation
if (commStatus == COMM_TXSUCCESS)
{
// Min size for an RX packet is 9
rxPacketSize = 9;
rxPacketSizeReceived = 0;
}
// Receive packet
int nRead = 0;
if (serial != NULL)
{
nRead = serial->rx((unsigned char*)&rxPacket[rxPacketSizeReceived], rxPacketSize - rxPacketSizeReceived);
rxPacketSizeReceived += nRead;
// Check if we received the whole packet
if (rxPacketSizeReceived < rxPacketSize)
{
if (serial->checkTimeOut() == 1)
{
if (rxPacketSizeReceived == 0)
{
commStatus = COMM_RXTIMEOUT;
}
else
{
commStatus = COMM_RXCORRUPT;
}
commLock = 0;
return;
}
}
}
else
{
TRACE_ERROR(HKX, "Serial interface has been destroyed!\n");
return;
}
// Find packet header
unsigned char i, j;
for (i = 0; i < (rxPacketSizeReceived - 1); i++)
{
if (rxPacket[i] == 0xFF &&
rxPacket[i+1] == 0xFF)
{
break;
}
else if (i == (rxPacketSizeReceived - 2) &&
rxPacket[rxPacketSizeReceived-1] == 0xFF)
{
break;
}
}
if (i > 0)
{
for (j = 0; j < (rxPacketSizeReceived-i); j++)
{
rxPacket[j] = rxPacket[j + i];
}
rxPacketSizeReceived -= i;
}
// Incomplete packet?
if (rxPacketSizeReceived < rxPacketSize)
{
commStatus = COMM_RXWAITING;
return;
}
// Check ID pairing
if (txPacket[PKT_ID] != rxPacket[PKT_ID])
{
commStatus = COMM_RXCORRUPT;
commLock = 0;
return;
}
// Rx packet size
rxPacketSize = hkx_get_rxpacket_size();
if (rxPacketSizeReceived < rxPacketSize)
{
nRead = serial->rx(&rxPacket[rxPacketSizeReceived], rxPacketSize - rxPacketSizeReceived);
rxPacketSizeReceived += nRead;
if (rxPacketSizeReceived < rxPacketSize)
{
commStatus = COMM_RXWAITING;
return;
}
}
// Generate a checksum of the incoming packet
{
unsigned short checksum = hkx_checksum_packet(rxPacket, rxPacketSize);
// Compare it with the internal packet checksum
if (rxPacket[PKT_CHECKSUM1] != get_lowbyte(checksum) &&
rxPacket[PKT_CHECKSUM2] != get_highbyte(checksum))
{
commStatus = COMM_RXCORRUPT;
commLock = 0;
return;
}
}
commStatus = COMM_RXSUCCESS;
commLock = 0;
}
void HerkuleX::hkx_tx_packet()
{
if (serial == NULL)
{
TRACE_ERROR(HKX, "Serial interface is not initialized!\n");
return;
}
if (commLock == 1)
{
return;
}
commLock = 1;
// Make sure serial link is "clean"
if (commStatus == COMM_RXTIMEOUT || commStatus == COMM_RXCORRUPT)
{
serial->flush();
}
// Make sure the packet is properly formed
if (hkx_validate_packet() == 0)
{
return;
}
int txPacketSize = hkx_get_txpacket_size();
unsigned char txPacketSizeSent = 0;
// Generate a checksum and write it into the last two bytes of the packet
unsigned short crc = hkx_checksum_packet(txPacket, txPacketSize);
txPacket[PKT_CHECKSUM1] = get_lowbyte(crc);
txPacket[PKT_CHECKSUM2] = get_highbyte(crc);
// Send packet
if (serial != NULL)
{
txPacketSizeSent = serial->tx(txPacket, txPacketSize);
}
else
{
TRACE_ERROR(HKX, "Serial interface has been destroyed!\n");
return;
}
// Check if we send the whole packet
if (txPacketSize != txPacketSizeSent)
{
commStatus = COMM_TXFAIL;
commLock = 0;
return;
}
// Set a timeout for the response packet
// Min size for an RX packet is 9
if (txPacket[PKT_CMD] == CMD_EEP_READ || txPacket[PKT_CMD] == CMD_RAM_READ)
{
serial->setTimeOut(9 + txPacket[PKT_DATA+1]);
}
else
{
serial->setTimeOut(9);
}
commStatus = COMM_TXSUCCESS;
}
