int Dynamixel::bulkRead(byte *param, int param_length) {
//mResult = 0;
uint32 bulkReadlength=0;
int n, i, k=0;
int num = param_length / 5; // each length : 5 (ID ADDR_L ADDR_H LEN_L LEN_H)
// int pkt_length = param_length + 3; // 3 : INST CHKSUM_L CHKSUM_H
// unsigned char txpacket[MAXNUM_TXPACKET] = {0};
// unsigned char rxpacket[MAXNUM_RXPACKET] = {0};
for(n=0; n < param_length; n++) {
mParamBuffer[n] = param[n];
}
/************ TxRxPacket *************/
// Wait for Bus Idle
/* while(comm->iBusUsing == 1)
{
//Sleep(0);
}*/
//mResult = txrx_PacketEx(BROADCAST_ID, INST_BULK_READ_EX, param_length);;
this->txRxPacket(BROADCAST_ID, INST_BULK_READ, param_length);
for(n = 0; n < num; n++) {
// int id = param[n*5+0];
bulkReadlength = this->rxPacket(param_length+11);
/*result = DXL_MAKEDWORD( DXL_MAKEWORD(gbpRxBufferEx[9],gbpRxBufferEx[10]),
DXL_MAKEWORD(gbpRxBufferEx[11],gbpRxBufferEx[12])
);*/
if(mRxBuffer[7] == 0x55) { //packet instruction index
mBulkData[n].iID = mRxBuffer[4]; //packet ID index
mBulkData[n].iAddr = DXL_MAKEWORD(mParamBuffer[5*n+1],mParamBuffer[5*n+2]); //get address
mBulkData[n].iLength = DXL_MAKEWORD(mParamBuffer[5*n+3],mParamBuffer[5*n+4]);//DXL_MAKEWORD(gbpRxBufferEx[PKT_LENGTH_L],gbpRxBufferEx[PKT_LENGTH_H]);
//TxDStringC("iLength = ");TxDHex8C(mBulkData[n].iLength);TxDStringC("\r\n");
mBulkData[n].iError = mRxBuffer[7+1]; //Error code
for(i=0; i < mBulkData[n].iLength ; i++) {
mBulkData[n].iData[i] = mRxBuffer[7+2+i]; //DATA1
}
}
for(k=0; k < DXL_RX_BUF_SIZE ; k++) {
mRxBuffer[k] = 0; //buffer clear
}
this->clearBuffer();
}
return bulkReadlength;
}
uint32 Dynamixel::readDword( byte bID, word wAddress ) {
if(mPacketType == DXL_PACKET_TYPE1) return 0xFFFFFFFF;
mParamBuffer[0] = (unsigned char)DXL_LOBYTE(wAddress);
mParamBuffer[1] = (unsigned char)DXL_HIBYTE(wAddress);
mParamBuffer[2] = 4; //4byte
mParamBuffer[3] = 0;
if(this->txRxPacket(bID, INST_READ, 4)) {
return DXL_MAKEDWORD( DXL_MAKEWORD( mRxBuffer[9], mRxBuffer[10]),
DXL_MAKEWORD( mRxBuffer[11], mRxBuffer[12]));
} else {
return 0xFFFFFFFF;
}
}
uint16_t pingGetModelNum1(int port_num, uint8_t id)
{
packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 2 * sizeof(uint8_t));
packetData[port_num].communication_result = COMM_TX_FAIL;
packetData[port_num].tx_packet = (uint8_t *)realloc(packetData[port_num].tx_packet, 6);
packetData[port_num].rx_packet = (uint8_t *)realloc(packetData[port_num].rx_packet, 6);
if (id >= BROADCAST_ID)
{
packetData[port_num].communication_result = COMM_NOT_AVAILABLE;
return 0;
}
packetData[port_num].tx_packet[PKT_ID] = id;
packetData[port_num].tx_packet[PKT_LENGTH] = 2;
packetData[port_num].tx_packet[PKT_INSTRUCTION] = INST_PING;
txRxPacket1(port_num);
if (packetData[port_num].communication_result == COMM_SUCCESS)
{
readTxRx1(port_num, id, 0, 2); // Address 0 : Model Number
if (packetData[port_num].communication_result == COMM_SUCCESS)
return DXL_MAKEWORD(packetData[port_num].data_read[0], packetData[port_num].data_read[1]);
}
return 0;
}
UINT16_T PingGetModelNum1(int port_num, UINT8_T id)
{
packetData[port_num].data_read_ = (UINT8_T *)realloc(packetData[port_num].data_read_, 2 * sizeof(UINT8_T));
packetData[port_num].communication_result_ = COMM_TX_FAIL;
packetData[port_num].txpacket_ = (UINT8_T *)realloc(packetData[port_num].txpacket_, 6);
packetData[port_num].rxpacket_ = (UINT8_T *)realloc(packetData[port_num].rxpacket_, 6);
if (id >= BROADCAST_ID)
{
packetData[port_num].communication_result_ = COMM_NOT_AVAILABLE;
return 0;
}
packetData[port_num].txpacket_[PKT_ID] = id;
packetData[port_num].txpacket_[PKT_LENGTH] = 2;
packetData[port_num].txpacket_[PKT_INSTRUCTION] = INST_PING;
TxRxPacket1(port_num);
if (packetData[port_num].communication_result_ == COMM_SUCCESS)
{
ReadTxRx1(port_num, id, 0, 2); // Address 0 : Model Number
if (packetData[port_num].communication_result_ == COMM_SUCCESS)
return DXL_MAKEWORD(packetData[port_num].data_read_[0], packetData[port_num].data_read_[1]);
}
return 0;
}
word Dynamixel::ping(byte bID ) {
if(this->txRxPacket(bID, INST_PING, 0)) {
if(mPacketType == DXL_PACKET_TYPE1) return (mRxBuffer[2]); //1.0
else return DXL_MAKEWORD(mRxBuffer[9],mRxBuffer[10]); //return product code when 2.0
} else {
return 0xffff; //no dxl in bus.
}
}
uint16_t read2ByteRx1(int port_num)
{
packetData[port_num].data_read = (uint8_t *)realloc(packetData[port_num].data_read, 2 * sizeof(uint8_t));
packetData[port_num].data_read[0] = 0;
packetData[port_num].data_read[1] = 0;
readRx1(port_num, 2);
if (packetData[port_num].communication_result == COMM_SUCCESS)
return DXL_MAKEWORD(packetData[port_num].data_read[0], packetData[port_num].data_read[1]);
return 0;
}
UINT16_T Read2ByteRx1(int port_num)
{
packetData[port_num].data_read_ = (UINT8_T *)realloc(packetData[port_num].data_read_, 2 * sizeof(UINT8_T));
packetData[port_num].data_read_[0] = 0;
packetData[port_num].data_read_[1] = 0;
ReadRx1(port_num, 2);
if (packetData[port_num].communication_result_ == COMM_SUCCESS)
return DXL_MAKEWORD(packetData[port_num].data_read_[0], packetData[port_num].data_read_[1]);
return 0;
}
word Dynamixel::readWord(byte bID, word bAddress) {
this->clearBuffer();
if(mPacketType == DXL_PACKET_TYPE1) {
mParamBuffer[0] = bAddress;
mParamBuffer[1] = 2;
if(this->txRxPacket(bID, INST_READ, 2)) {
return DXL_MAKEWORD(mRxBuffer[5],mRxBuffer[6]);//( (((word)mRxBuffer[6])<<8)+ mRxBuffer[5] );
}
else {
return 0xffff;
}
} else {
mParamBuffer[0] = (unsigned char)DXL_LOBYTE(bAddress);
mParamBuffer[1] = (unsigned char)DXL_HIBYTE(bAddress);
mParamBuffer[2] = 2; //2byte
mParamBuffer[3] = 0;
if(this->txRxPacket(bID, INST_READ, 4)) {
return(DXL_MAKEWORD(mRxBuffer[9], mRxBuffer[10]));
} else {
return 0xffff;
}
}
}
uint32_t getDataRead1(int port_num, uint16_t data_length, uint16_t data_pos)
{
switch (data_length)
{
case 1:
return packetData[port_num].data_read[data_pos + 0];
case 2:
return DXL_MAKEWORD(packetData[port_num].data_read[data_pos + 0], packetData[port_num].data_read[data_pos + 1]);
default:
printf("[Set Data Read] failed... ");
return 0;
}
}
byte Dynamixel::txRxPacket(byte bID, byte bInst, int bTxParaLen) {
mDXLtxrxStatus = 0;
word bTxLen, bRxLenEx, bTryCount;
mBusUsed = 1;
mRxLength = bRxLenEx = bTxLen = 0;
for(bTryCount = 0; bTryCount < gbDXLNumberTxRxAttempts; bTryCount++)//for(bTryCount = 0; bTryCount < TRY_NUM; bTryCount++)
{
//gbDXLReadPointer = gbDXLWritePointer;
mDxlDevice->read_pointer = mDxlDevice->write_pointer;//[ROBOTIS]BufferClear050728
/************************************** Transfer packet ***************************************************/
bTxLen = this->txPacket(bID, bInst, bTxParaLen);
if(mPacketType == DXL_PACKET_TYPE1) { //Dxl 1.0 Tx success ?
if (bTxLen == (bTxParaLen+4+2)) mDXLtxrxStatus = (1<<COMM_TXSUCCESS);
} else { //Dxl 2.0 Tx success?
if (bTxLen == (bTxParaLen+3+7)) mDXLtxrxStatus = (1<<COMM_TXSUCCESS);
}
//TxDStringC("bTxLen = ");TxDHex8C(bTxLen);TxDStringC("\r\n");
if(bInst == INST_PING) {
if(mPacketType == DXL_PACKET_TYPE1) { //Dxl 1.0
if(bID == BROADCAST_ID) mRxLength = bRxLenEx = 0xff;
else mRxLength = bRxLenEx = 6; // basic response packet length
} else { //Dxl 2.0
if(bID == BROADCAST_ID) mRxLength = bRxLenEx = 0xffff;
else mRxLength = bRxLenEx = 14;
}
}
else if(bInst == INST_READ) {
if (gbDXLStatusReturnLevel > 0) {
if(mPacketType == DXL_PACKET_TYPE1) mRxLength = bRxLenEx = 6+mParamBuffer[1];
else mRxLength = bRxLenEx = 11+DXL_MAKEWORD(mParamBuffer[2], mParamBuffer[3]);
}
else {
mRxLength = bRxLenEx = 0;
}
}
else if( bID == BROADCAST_ID ) {
if(bInst == INST_SYNC_READ || bInst == INST_BULK_READ) mRxLength = bRxLenEx = 0xffff; //only 2.0 case
else mRxLength = bRxLenEx = 0; // no response packet
}
else {
if (gbDXLStatusReturnLevel>1) {
if(mPacketType == DXL_PACKET_TYPE1) mRxLength = bRxLenEx = 6;//+mParamBuffer[1];
else mRxLength = bRxLenEx = 11;
}
else {
mRxLength = bRxLenEx = 0;
}
}
if(bRxLenEx) {
if(SmartDelayFlag == 1)
delay(150);
/************************************** Receive packet ***************************************************/
mRxLength = this->rxPacket(bRxLenEx);
}//bRxLenEx is exist
} //for() gbDXLNumberTxRxAttempts
//TxDStringC("\r\n TEST POINT 2");//TxDString("\r\n Err ID:0x");
mBusUsed = 0;
if((mRxLength != bRxLenEx) && (mTxBuffer[mPktIdIndex] != BROADCAST_ID))
{
//TxDByteC('3');//
//TxDStringC("Rx Error\r\n");//TxDString("\r\n Err ID:0x");
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
//TxDString("\r\n Err ID:0x");
//TxDHex8(bID);
TxDStringC("\r\n ->[DXL]Err: ");
printBuffer(mTxBuffer,bTxLen);
TxDStringC("\r\n <-[DXL]Err: ");
printBuffer(mRxBuffer,mRxLength);
#endif
#ifdef PRINT_OUT_TRACE_ERROR_PRINT_TO_USART2
//TxDString("\r\n {[ERROR:");TxD16Hex(0x8100);TxDByte(':');TxD16Hex(bID);TxDByte(':');TxD8Hex(bInst);TxDByte(']');TxDByte('}');
//TxDByte(bID);TxDByte(' ');
//TxDByte(bInst);TxDByte(' ');
//TxDByte(gbpParameter[0]);TxDByte(' ');
//TxDByte(gbpParameter[1]);TxDByte(' ');
#endif
return 0;
} else if((mRxLength == 0) && (mTxBuffer[mPktInstIndex] == INST_PING)) { //[ROBOTIS] 2013-11-22 correct response for ping instruction
//return 0;
}
//TxDString("\r\n TEST POINT 4");//TxDString("\r\n Err ID:0x");
#ifdef PRINT_OUT_PACKET_TO_USART2
TxDStringC("\r\n ->[TX Buffer]: ");
printBuffer(mTxBuffer,bTxLen);
TxDStringC("\r\n <-[RX Buffer]: ");
printBuffer(mRxBuffer,mRxLength);
#endif
mDXLtxrxStatus = (1<<COMM_RXSUCCESS);
//gbLengthForPacketMaking =0;
return 1;
}
byte Dynamixel::rxPacket(int bRxLength) {
unsigned long ulCounter, ulTimeLimit;
word bCount, bLength, bChecksum;
byte bTimeout;
bTimeout = 0;
if(bRxLength == 255 || bRxLength == 0xffff) //2014-04-03
ulTimeLimit = RX_TIMEOUT_COUNT1;
else
ulTimeLimit = RX_TIMEOUT_COUNT2;
for(bCount = 0; bCount < bRxLength; bCount++)
{
ulCounter = 0;
while(mDxlDevice->read_pointer == mDxlDevice->write_pointer)
{
nDelay(NANO_TIME_DELAY); //[ROBOTIS] porting ydh
if(ulCounter++ > ulTimeLimit)
{
bTimeout = 1;
//TxDStringC("Timeout\r\n");
break;
}
uDelay(0); //[ROBOTIS] porting ydh added //if exist DXL 1.0 -> ok DXL 2.0 -> ok, if not exist 1.0 not ok, 2.0 ok
}
if(bTimeout) break;
mRxBuffer[bCount] = mDxlDevice->data_buffer[mDxlDevice->read_pointer++ & DXL_RX_BUF_SIZE]; // get packet data from USART device
//TxDStringC("mRxBuffer = ");TxDHex8C(mRxBuffer[bCount]);TxDStringC("\r\n");
}
bLength = bCount;
bChecksum = 0;
if( mTxBuffer[mPktIdIndex] != BROADCAST_ID )
{
if(bTimeout && bRxLength != 255)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Rx Timeout");
TxDByteC(bLength);
#endif
mDXLtxrxStatus |= (1<<COMM_RXTIMEOUT);
clearBuffer();
//TxDStringC("Rx Timeout");
return 0;
}
if(bLength > 3) //checking available length.
{
/*if(mPacketType == 1){
if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff ) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER);
else if(mRxBuffer[mPktIdIndex] != mTxBuffer[mPktIdIndex] ) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID);
else if(mRxBuffer[mPktLengthIndex] != bLength-mPktInstIndex) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH);
else{
for(bCount = 2; bCount < bLength; bCount++){
bChecksum += mRxBuffer[bCount]; //Calculate checksum of received data for compare
}
if(bChecksum != 0xff) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM);
return 0;
}
}else{
if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff || mRxBuffer[2] != 0xfd) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER);
else if(mRxBuffer[mPktIdIndex] != mTxBuffer[mPktIdIndex] ) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID);
else if(mRxBuffer[mPktLengthIndex] != bLength-mPktInstIndex) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH);
else{
bChecksum = DXL_MAKEWORD(mRxBuffer[bRxLength-2], mRxBuffer[bRxLength-1]);
if(update_crc(0, mRxBuffer, bRxLength-2) != bChecksum) mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM);
return 0;
}
}
*/
if(mPacketType == 1) { //Dxl 1.0 header check
if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff ) {
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Wrong Header");//[Wrong Header]
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER);
clearBuffer();
return 0;
}
} else { // Dxl 2.0 header check
if(mRxBuffer[0] != 0xff || mRxBuffer[1] != 0xff || mRxBuffer[2] != 0xfd)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Wrong Header");//[Wrong Header]
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXHEADER);
clearBuffer();
return 0;
}
}
if(mRxBuffer[mPktIdIndex] != mTxBuffer[mPktIdIndex] ) //id check
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[Error:TxID != RxID]");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID);
clearBuffer();
return 0;
}
if(mRxBuffer[mPktLengthIndex] != bLength-mPktInstIndex) // status packet length check
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("RxLength Error");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH);
clearBuffer();
return 0;
}
if(mPacketType == 1 && mRxBuffer[mPktErrorIndex] != 0) { //140512 shin
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
for(bTryCount = 0; bTryCount<= 6; bTryCount++) {
if((mRxBuffer[mPktErrorIndex] & (1<<bTryCount)) == TRUE) {
switch(bTryCount) {
case 0:
TxDStringC("InputVoltage Error");
break;
case 1:
TxDStringC("Angle Limit Error");
break;
case 2:
TxDStringC("Overheating Error");
break;
case 3:
TxDStringC("Range Error");
break;
case 4:
TxDStringC("Checksum Error");
break;
case 5:
TxDStringC("Overload Error");
break;
case 6:
TxDStringC("Instruction Error");
break;
}
}
}
#endif
} else { //140512 shin
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
for(bTryCount = 1; bTryCount<= 7; bTryCount++) {
if((mRxBuffer[mPktErrorIndex]) == bTryCount) {
switch(bTryCount) {
case 1:
TxDStringC("Result Fail");
break;
case 2:
TxDStringC("Instruction Error");
break;
case 3:
TxDStringC("CRC Error");
break;
case 4:
TxDStringC("DataRange Error");
break;
case 5:
TxDStringC("DataLength Error");
break;
case 6:
TxDStringC("DataLimit Error");
break;
case 7:
TxDStringC("Accrss Error");
break;
}
}
}
#endif
}
if(mPacketType == 1) { // Dxl 1.0 checksum
for(bCount = 2; bCount < bLength; bCount++) {
bChecksum += mRxBuffer[bCount]; //Calculate checksum of received data for compare
}
if(bChecksum != 0xff)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[RxChksum Error]");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXCHECKSUM);
clearBuffer();
return 0;
}
} else { // Dxl 2.0 checksum
bChecksum = DXL_MAKEWORD(mRxBuffer[bRxLength-2], mRxBuffer[bRxLength-1]);
if(update_crc(0, mRxBuffer, bRxLength-2) == bChecksum) { // -2 : except CRC16
return bLength;
}
else {
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("CRC-16 Error\r\n");
#endif
return 0;
}
}//end of checksum
}//(bLength > 3)
}//end of Rx status packet check
return bLength;
}
