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