void readMessage(char *buffer, int sock) //parse the commands read
{
int readStatus;
zeroBuffer(buffer);
readStatus = read(sock, buffer, MAX_MESSAGE_SIZE);
printf("command read: %d\n", (int)buffer[COMMAND_LOCATION]);
switch ((int)buffer[COMMAND_LOCATION])
{
case PORT:
portHandler(buffer, sock);
break;
case USERNAME:
usernameHandler(buffer, sock);
break;
case DISCONNECT:
disconnectHandler(buffer, sock);
break;
case LIST_USERS:
listHandler(buffer, sock);
break;
case MESSAGE:
messageUserHandler(buffer, sock);
break;
default:
printf("\nCommand not supported: (%d)\n", buffer[COMMAND_LOCATION]);
writeMessageDefined(sock, buffer);
break;
}
if (readStatus < 0)
{
error("ERROR reading from socket");
}
else
{
if(!chop(buffer)) //remove any unwanted characters
{
memset(buffer, 0, MAX_MESSAGE_SIZE);
buffer = TIMEOUT_CALL;
}
}
}
int main()
{
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = portHandler(DEVICENAME);
// Initialize PacketHandler Structs
packetHandler();
// Initialize Groupbulkread Structs
int group_num = groupBulkRead(port_num, PROTOCOL_VERSION);
int index = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_addparam_result = False; // AddParam result
uint8_t dxl_getdata_result = False; // GetParam result
int dxl_goal_position[2] = { DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl1_present_position = 0; // Present position
uint8_t dxl2_moving = 0; // Dynamixel moving status
// Open port
if (openPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (setBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Enable Dynamixel#1 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
else
{
printf("Dynamixel#%d has been successfully connected \n", DXL1_ID);
}
// Enable Dynamixel#2 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
else
{
printf("Dynamixel#%d has been successfully connected \n", DXL2_ID);
}
// Add parameter storage for Dynamixel#1 present position value
dxl_addparam_result = groupBulkReadAddParam(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupBulkRead addparam failed", DXL1_ID);
return 0;
}
// Add parameter storage for Dynamixel#2 present moving value
dxl_addparam_result = groupBulkReadAddParam(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupBulkRead addparam failed", DXL2_ID);
return 0;
}
while (1)
{
printf("Press any key to continue! (or press ESC to quit!)\n");
if (getch() == ESC_ASCII_VALUE)
break;
// Write Dynamixel#1 goal position
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Write Dynamixel#2 goal position
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
do
{
// Bulkread present position and moving status
groupBulkReadTxRxPacket(group_num);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
dxl_getdata_result = groupBulkReadIsAvailable(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupBulkRead getdata failed", DXL1_ID);
return 0;
}
dxl_getdata_result = groupBulkReadIsAvailable(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupBulkRead getdata failed", DXL2_ID);
return 0;
}
// Get Dynamixel#1 present position value
dxl1_present_position = groupBulkReadGetData(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION);
// Get Dynamixel#2 moving status value
dxl2_moving = groupBulkReadGetData(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING);
printf("[ID:%03d] Present Position : %d \t [ID:%03d] Is Moving : %d\n", DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_moving);
} while (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD);
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel#1 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Disable Dynamixel#2 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Close port
closePort(port_num);
return 0;
}
int main()
{
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = portHandler(DEVICENAME);
// Initialize PacketHandler Structs
packetHandler();
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl_model_number; // Dynamixel model number
// Open port
if (openPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (setBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Try to ping the Dynamixel
// Get Dynamixel model number
dxl_model_number = pingGetModelNum(port_num, PROTOCOL_VERSION, DXL_ID);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printf("%s\n", getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printf("%s\n", getRxPacketError(PROTOCOL_VERSION, dxl_error));
}
else
{
printf("[ID:%03d] ping Succeeded. Dynamixel model number : %d\n", DXL_ID, dxl_model_number);
}
// Close port
closePort(port_num);
return 0;
}
int main()
{
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = portHandler(DEVICENAME);
// Initialize PacketHandler Structs
packetHandler();
int index = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
int dxl_goal_position[2] = { DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl_present_position = 0; // Present position
// Open port
if (openPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (setBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Enable Dynamixel Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
else
{
printf("Dynamixel has been successfully connected \n");
}
while (1)
{
printf("Press any key to continue! (or press ESC to quit!)\n");
if (getch() == ESC_ASCII_VALUE)
break;
// Write goal position
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
do
{
// Read present position
dxl_present_position = read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL_ID, dxl_goal_position[index], dxl_present_position);
} while ((abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD));
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Close port
closePort(port_num);
return 0;
}
int main()
{
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = portHandler(DEVICENAME);
// Initialize PacketHandler Structs
packetHandler();
// Initialize Groupsyncwrite instance
int groupwrite_num = groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE);
// Initialize Groupsyncread instance
int groupread_num = groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ);
int index = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_addparam_result = False; // AddParam result
uint8_t dxl_getdata_result = False; // GetParam result
int dxl_goal_position[2] = { DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position
uint8_t dxl_error = 0; // Dynamixel error
uint8_t dxl_moving = 0; // Dynamixel moving status
uint8_t dxl_led_value[2] = { 0x00, 0xFF }; // Dynamixel LED value
int32_t dxl_present_position = 0; // Present position
// Open port
if (openPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate
if (setBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Disable Dynamixel Torque :
// Indirect address would not accessible when the torque is already enabled
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
else
{
printf("DXL has been successfully connected \n");
}
// INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Enable Dynamixel Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Add parameter storage for the present position value
dxl_addparam_result = groupSyncReadAddParam(groupread_num, DXL_ID);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead addparam failed\n", DXL_ID);
return 0;
}
while (1)
{
printf("Press any key to continue! (or press ESC to quit!)\n");
if (getch() == ESC_ASCII_VALUE)
break;
// Add values to the Syncwrite storage
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_goal_position[index], 4);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed\n", DXL_ID);
return 0;
}
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_led_value[index], 1);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed\n", DXL_ID);
return 0;
}
// Syncwrite all
groupSyncWriteTxPacket(groupwrite_num);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
// Clear syncwrite parameter storage
groupSyncWriteClearParam(groupwrite_num);
do
{
// Syncread present position from indirectdata2
groupSyncReadTxRxPacket(groupread_num);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
// Check if groupsyncread data of Dyanamixel is available
dxl_getdata_result = groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL_ID);
return 0;
}
// Check if groupsyncread data of Dyanamixel is available
dxl_getdata_result = groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL_ID);
return 0;
}
// Get Dynamixel present position value
dxl_present_position = groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION);
// Get Dynamixel moving status value
dxl_moving = groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING);
printf("[ID:%03d] GoalPos:%d PresPos:%d IsMoving:%d\n", DXL_ID, dxl_goal_position[index], dxl_present_position, dxl_moving);
} while (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD);
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Close port
closePort(port_num);
return 0;
}
