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::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::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; }