void Dynamixel::printBuffer(byte *bpPrintBuffer, byte bLength)
{
#ifdef PRINT_OUT_TRACE_ERROR_PRINT_TO_USART2
byte bCount;
if(bLength == 0)
{
if(mTxBuffer[2] == BROADCAST_ID)
{
TxDStringC("\r\n No Data[at Broadcast ID 0xFE]");
}
else
{
TxDStringC("\r\n No Data(Check ID, Operating Mode, Baud rate)");//TxDString("\r\n No Data(Check ID, Operating Mode, Baud rate)");
}
}
for(bCount = 0; bCount < bLength; bCount++)
{
TxDHex8C(bpPrintBuffer[bCount]);
TxDByteC(' ');
}
TxDStringC(" LEN:");//("(LEN:")
TxDHex8C(bLength);
TxDStringC("\r\n");
#endif
}
void PrintBuffer(byte *bpPrintBuffer, byte bLength)
{
byte bCount;
if(bLength == 0)
{
if(gbpTxBuffer[2] == BROADCAST_ID)
{
TxDStringC("\r\n No Data[at Broadcast ID 0xFE]");
}
else
{
TxDStringC("\r\n No Data(Check ID, Operating Mode, Baud rate)");//TxDString("\r\n No Data(Check ID, Operating Mode, Baud rate)");
}
}
for(bCount = 0; bCount < bLength; bCount++)
{
TxDHex8C(bpPrintBuffer[bCount]);
TxDByteC(' ');
}
TxDStringC(" LEN:");//("(LEN:")
TxDHex8C(bLength);
TxDStringC("\r\n");
}
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;
}
byte Dynamixel::txRxPacket(byte bID, byte bInst, byte bTxParaLen){
//#define TRY_NUM 2//;;2
mDXLtxrxStatus = 0;
byte bTxLen, bRxLenEx, bTryCount;
mBusUsed = 1;
mRxLength = bRxLenEx = 0;
for(bTryCount = 0; bTryCount < gbDXLNumberTxRxAttempts; bTryCount++)//for(bTryCount = 0; bTryCount < TRY_NUM; bTryCount++)
{
//gbDXLReadPointer = gbDXLWritePointer;
mDxlDevice->read_pointer = mDxlDevice->write_pointer;//[ROBOTIS]BufferClear050728
bTxLen = this->txPacket(bID, bInst, bTxParaLen);
if (bTxLen == (bTxParaLen+4+2)){
mDXLtxrxStatus = (1<<COMM_TXSUCCESS);
}
// else{
// return 0;
// }
if(bInst == INST_PING){
if(bID == BROADCAST_ID){
mRxLength = bRxLenEx = 255; //
}
else{
mRxLength = bRxLenEx = 6; // basic response packet length
}
}
else if(bInst == INST_READ){
//mRxLength = bRxLenEx = 6+mParamBuffer[1]; // basic response packet length + requested data length in read instruction
if (gbDXLStatusReturnLevel>0)
mRxLength = bRxLenEx = 6+mParamBuffer[1];
else
mRxLength = bRxLenEx = 0;
}
else if( bID == BROADCAST_ID ){
mRxLength = bRxLenEx = 0; // no response packet in case broadcast id
break;
}
else{
//mRxLength = bRxLenEx = 6; //basic response packet length
if (gbDXLStatusReturnLevel>1)
mRxLength = bRxLenEx = 6+mParamBuffer[1];
else
mRxLength = bRxLenEx = 0;
}
if(bRxLenEx){//(gbpValidServo[bID] > 0x81 || bInst != INST_WRITE)) //ValidServo = 0x80|RETURN_LEVEL
mRxLength = this->rxPacket(bRxLenEx);
//TxDStringC("gbRxLength = ");TxDHex8C(mRxLength);TxDStringC("\r\n");
//TxDStringC("bRxLenEx = ");TxDHex8C(bRxLenEx);TxDStringC("\r\n");
// if(gbRxLength != bRxLenEx) //&& bRxLenEx != 255) before Ver 1.11e
if((mRxLength != bRxLenEx) && (bRxLenEx != 255)) // after Ver 1.11f
{
//TxDStringC(" Length mismatch!!\r\n");
unsigned long ulCounter;
word wTimeoutCount;
ulCounter = 0;
wTimeoutCount = 0;
//TxDStringC("\r\n TEST POINT 0");
while(ulCounter++ < RX_TIMEOUT_COUNT2)
{
//if(gbDXLReadPointer != gbDXLWritePointer)
if(this->available()) //data is available in dxl bus
{
mDxlDevice->read_pointer = mDxlDevice->write_pointer;// gbDXLWritePointer; //BufferClear
ulCounter = 0;
if(wTimeoutCount++ > 100 )
{
//uDelay(0);// porting ydh added
break; //porting ydh 100->245 //;;;;;; min max µÚ¹Ù²ñ found at Ver 1.11e
}
nDelay(NANO_TIME_DELAY);// porting ydh added 20120210.
}
//uDelay(0);// porting ydh added
nDelay(NANO_TIME_DELAY);// porting ydh added
}
//TxDStringC("\r\n TEST POINT 111");
mDxlDevice->read_pointer = mDxlDevice->write_pointer; //BufferClear
}
else{
//TxDStringC("\r\n TEST POINT 6");
break;
}
}//bRxLenEx is exist
}
//TxDStringC("\r\n TEST POINT 2");//TxDString("\r\n Err ID:0x");
mBusUsed = 0;
if((mRxLength != bRxLenEx) && (mTxBuffer[2] != 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[4] == 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(byte bRxLength){
unsigned long ulCounter, ulTimeLimit;
byte bCount, bLength, bChecksum;
byte bTimeout;
bTimeout = 0;
if(bRxLength == 255)
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(bTimeout) break;
mRxBuffer[bCount] = mDxlDevice->data_buffer[mDxlDevice->read_pointer++]; // get packet data from USART device
//TxDStringC("mRxBuffer = ");TxDHex8C(mRxBuffer[bCount]);TxDStringC("\r\n");
}
bLength = bCount;
bChecksum = 0;
if( mTxBuffer[2] != 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(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;
}
if(mRxBuffer[2] != mTxBuffer[2] )
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[Error:TxID != RxID]");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXID);
clearBuffer();
return 0;
}
if(mRxBuffer[3] != bLength-4)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("RxLength Error");
#endif
mDXLtxrxStatus |= (1<<COMM_RXCORRUPT);//RXLENGTH);
clearBuffer();
return 0;
}
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;
}
}
}
return bLength;
}
//갯수에 맞게 받는 이유 : 통신에러가 나오면 Length가 틀릴 가능성이 무척 높기 때문
byte rx_Packet(byte bRxLength){
unsigned long ulCounter, ulTimeLimit;
byte bCount, bLength, bChecksum;
byte bTimeout;
bTimeout = 0;
if(bRxLength == 255) ulTimeLimit = RX_TIMEOUT_COUNT1;
else ulTimeLimit = RX_TIMEOUT_COUNT2;
for(bCount = 0; bCount < bRxLength; bCount++)
{
ulCounter = 0;
while(gbDXLReadPointer == gbDXLWritePointer)
{
nDelay(NANO_TIME_DELAY); // porting ydh
if(ulCounter++ > ulTimeLimit)
{
bTimeout = 1;
break;
}
uDelay(0); //porting ydh added
}
if(bTimeout) break;
gbpRxBuffer[bCount] = gbpDXLDataBuffer[gbDXLReadPointer++];
//TxDStringC("gbpRxBuffer = ");TxDHex8C(gbpRxBuffer[bCount]);TxDStringC("\r\n");
}
bLength = bCount;
bChecksum = 0;
if( gbpTxBuffer[2] != BROADCAST_ID )
{
if(bTimeout && bRxLength != 255)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDString("Rx Timeout");
TxDByte(bLength);
#endif
clearBuffer256();
//return 0;
}
if(bLength > 3) //checking available length.
{
if(gbpRxBuffer[0] != 0xff || gbpRxBuffer[1] != 0xff )
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("Wrong Header");//[Wrong Header]
#endif
clearBuffer256();
return 0;
}
if(gbpRxBuffer[2] != gbpTxBuffer[2] )
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[Error:TxID != RxID]");
#endif
clearBuffer256();
return 0;
}
if(gbpRxBuffer[3] != bLength-4)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("RxLength Error");
#endif
clearBuffer256();
return 0;
}
for(bCount = 2; bCount < bLength; bCount++) bChecksum += gbpRxBuffer[bCount];
if(bChecksum != 0xff)
{
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
TxDStringC("[RxChksum Error]");
#endif
clearBuffer256();
return 0;
}
}
}
return bLength;
}
byte txrx_Packet(byte bID, byte bInst, byte bTxParaLen){
#define TRY_NUM 2//;;2
byte bTxLen, bRxLenEx, bTryCount;
gbBusUsed = 1;
for(bTryCount = 0; bTryCount < TRY_NUM; bTryCount++)
{
gbDXLReadPointer = gbDXLWritePointer; //BufferClear050728
bTxLen = tx_Packet(bID, bInst, bTxParaLen);
if(bInst == INST_PING)
{
if(bID == BROADCAST_ID)
{
gbRxLength = bRxLenEx = 255;
}
else
{
gbRxLength = bRxLenEx = 6;
}
}
else if(bInst == INST_READ)
{
gbRxLength = bRxLenEx = 6+gbpParameter[1];
}
else if( bID == BROADCAST_ID )
{
gbRxLength = bRxLenEx = 0;
break;
}
else
{
gbRxLength = bRxLenEx = 6;
}
if(bRxLenEx)//(gbpValidServo[bID] > 0x81 || bInst != INST_WRITE)) //ValidServo = 0x80|RETURN_LEVEL
{
gbRxLength = rx_Packet(bRxLenEx);
//TxDStringC("gbRxLength = ");TxD_Dec_U8C(gbRxLength);TxDStringC("\r\n");
//TxDStringC("bRxLenEx = ");TxD_Dec_U8C(bRxLenEx);TxDStringC("\r\n");
// if(gbRxLength != bRxLenEx) //&& bRxLenEx != 255) before Ver 1.11e
if((gbRxLength != bRxLenEx) && (bRxLenEx != 255)) // after Ver 1.11f
{
unsigned long ulCounter;
word wTimeoutCount;
ulCounter = 0;
wTimeoutCount = 0;
//TxDByteC('0');//TxDStringC("\r\n TEST POINT 0");//TxDString("\r\n Err ID:0x");
while(ulCounter++ < RX_TIMEOUT_COUNT2)
{
if(gbDXLReadPointer != gbDXLWritePointer) {
gbDXLReadPointer = gbDXLWritePointer; //BufferClear
ulCounter = 0;
if(wTimeoutCount++ > 100 )
{
//uDelay(0);// porting ydh added
break; //porting ydh 100->245 //;;;;;; min max 뒤바뀜 found at Ver 1.11e
}
nDelay(NANO_TIME_DELAY);// porting ydh added 20120210.
}
//uDelay(0);// porting ydh added
nDelay(NANO_TIME_DELAY);// porting ydh added
}
//TxDByteC('1');//TxDStringC("\r\n TEST POINT 1");//TxDString("\r\n Err ID:0x");
gbDXLReadPointer = gbDXLWritePointer; //BufferClear
}
else
{
//TxDByteC('6');//TxDStringC("\r\n TEST POINT 6");//TxDString("\r\n Err ID:0x");
break;
}
}
}
//TxDByteC('2');//TxDStringC("\r\n TEST POINT 2");//TxDString("\r\n Err ID:0x");
gbBusUsed = 0;
if((gbRxLength != bRxLenEx) && (gbpTxBuffer[2] != BROADCAST_ID))
{
//TxDByteC('3');//TxDStringC("\r\n TEST POINT 3");//TxDString("\r\n Err ID:0x");
#ifdef PRINT_OUT_COMMUNICATION_ERROR_TO_USART2
//TxDString("\r\n Err ID:0x");
//TxDHex8(bID);
//TxDString("\r\n ->[DXL]Err: ");
PrintBuffer(gbpTxBuffer,bTxLen);
//TxDString("\r\n <-[DXL]Err: ");
PrintBuffer(gbpRxBuffer,gbRxLength);
#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;
}
//TxDString("\r\n TEST POINT 4");//TxDString("\r\n Err ID:0x");
#ifdef PRINT_OUT_PACKET_TO_USART2
TxDStringC("\r\n ->[TX Buffer]: ");
PrintBuffer(gbpTxBuffer,bTxLen);
TxDStringC("\r\n <-[RX Buffer]: ");
PrintBuffer(gbpRxBuffer,gbRxLength);
#endif
//gbLengthForPacketMaking =0;
return 1;
}
