void test() {
bMoving = dxl_read_byte( id, MOVING );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
if( bMoving == 0 )
{
// Change goal position
if( INDEX == 0 )
INDEX = 1;
else
INDEX = 0;
// Write goal position
dxl_write_word( id, GOAL_POSITION_L, GoalPos[INDEX] );
}
PrintErrorCode();
// Read present position
wPresentPos = dxl_read_word( id, PRESENT_POSITION_L );
TxDWord16(GoalPos[INDEX]);
TxDString(" ");
TxDWord16(wPresentPos);
TxDByte_PC('\r');
TxDByte_PC('\n');
}
else
PrintCommStatus(CommStatus);
}
// Print communication result
void PrintCommStatus(int CommStatus)
{
switch(CommStatus)
{
case COMM_TXFAIL:
TxDString("COMM_TXFAIL: Failed transmit instruction packet!\n");
break;
case COMM_TXERROR:
TxDString("COMM_TXERROR: Incorrect instruction packet!\n");
break;
case COMM_RXFAIL:
TxDString("COMM_RXFAIL: Failed get status packet from device!\n");
break;
case COMM_RXWAITING:
TxDString("COMM_RXWAITING: Now recieving status packet!\n");
break;
case COMM_RXTIMEOUT:
TxDString("COMM_RXTIMEOUT: There is no status packet!\n");
break;
case COMM_RXCORRUPT:
TxDString("COMM_RXCORRUPT: Incorrect status packet!\n");
break;
default:
TxDString("This is unknown error code!\n");
break;
}
}
int main(void)
{
/* System Clocks Configuration */
RCC_Configuration();
/* NVIC configuration */
NVIC_Configuration();
/* GPIO configuration */
GPIO_Configuration();
SysTick_Configuration();
Timer_Configuration();
dxl_initialize( 0, 1 );
USART_Configuration(USART_PC, Baudrate_PC);
while(1)
{
bMoving = dxl_read_byte( id, P_MOVING );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
if( bMoving == 0 )
{
// Change goal position
if( INDEX == 0 )
INDEX = 1;
else
INDEX = 0;
// Write goal position
dxl_write_word( id, P_GOAL_POSITION_L, GoalPos[INDEX] );
}
PrintErrorCode();
// Read present position
wPresentPos = dxl_read_word( id, P_PRESENT_POSITION_L );
TxDWord16(GoalPos[INDEX]);
TxDString(" ");
TxDWord16(wPresentPos);
TxDByte_PC('\r');
TxDByte_PC('\n');
}
else
PrintCommStatus(CommStatus);
}
return 0;
}
// Print error bit of status packet
void PrintErrorCode()
{
if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
TxDString("Input voltage error!\n");
if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
TxDString("Angle limit error!\n");
if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
TxDString("Overheat error!\n");
if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
TxDString("Out of range error!\n");
if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
TxDString("Checksum error!\n");
if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
TxDString("Overload error!\n");
if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
TxDString("Instruction code error!\n");
}
//갯수에 맞게 받는 이유 : 통신에러가 나오면 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;
}
