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