int main()
{
// Initialize PortHandler instance
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
// Initialize GroupSyncWrite instance
dynamixel::GroupSyncWrite groupSyncWrite(portHandler, packetHandler, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE);
// Initialize Groupsyncread instance
dynamixel::GroupSyncRead groupSyncRead(portHandler, packetHandler, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ);
int index = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
bool dxl_addparam_result = false; // addParam result
bool 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 param_indirect_data_for_write[LEN_PRO_INDIRECTDATA_FOR_WRITE];
uint8_t dxl_led_value[2] = {0x00, 0xFF}; // Dynamixel LED value
int32_t dxl_present_position = 0; // Present position
// Open port
if (portHandler->openPort())
{
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 (portHandler->setBaudRate(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
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
else
{
printf("DXL has been successfully connected \n");
}
// INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
// Add parameter storage for the present position value
dxl_addparam_result = groupSyncRead.addParam(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;
// Allocate LED and goal position value into byte array
param_indirect_data_for_write[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_indirect_data_for_write[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_indirect_data_for_write[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index]));
param_indirect_data_for_write[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]));
param_indirect_data_for_write[4] = dxl_led_value[index];
// Add values to the Syncwrite storage
dxl_addparam_result = groupSyncWrite.addParam(DXL_ID, param_indirect_data_for_write);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed\n", DXL_ID);
return 0;
}
// Syncwrite all
dxl_comm_result = groupSyncWrite.txPacket();
if (dxl_comm_result != COMM_SUCCESS) printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
// Clear syncwrite parameter storage
groupSyncWrite.clearParam();
do
{
// Syncread present position from indirectdata2
dxl_comm_result = groupSyncRead.txRxPacket();
if (dxl_comm_result != COMM_SUCCESS) printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
// Check if groupsyncread data of Dyanamixel is available
dxl_getdata_result = groupSyncRead.isAvailable(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 = groupSyncRead.isAvailable(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 = groupSyncRead.getData(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION);
// Get Dynamixel moving status value
dxl_moving = groupSyncRead.getData(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
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
printf("%s\n", packetHandler->getRxPacketError(dxl_error));
}
// Close port
portHandler->closePort();
return 0;
}
int main()
{
// Initialize PortHandler instance
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
// Initialize GroupSyncWrite instance
dynamixel::GroupSyncWrite groupSyncWrite(portHandler, packetHandler, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION);
// Initialize Groupsyncread instance for Present Position
dynamixel::GroupSyncRead groupSyncRead(portHandler, packetHandler, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
int index = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
bool dxl_addparam_result = false; // addParam result
bool 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 param_goal_position[4];
int32_t dxl1_present_position = 0, dxl2_present_position = 0; // Present position
// Open port
if (portHandler->openPort())
{
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 (portHandler->setBaudRate(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
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->printTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
else
{
printf("Dynamixel#%d has been successfully connected \n", DXL1_ID);
}
// Enable Dynamixel#2 Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->printTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(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 = groupSyncRead.addParam(DXL1_ID);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncRead addparam failed", DXL1_ID);
return 0;
}
// Add parameter storage for Dynamixel#2 present position value
dxl_addparam_result = groupSyncRead.addParam(DXL2_ID);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncRead 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;
// Allocate goal position value into byte array
param_goal_position[0] = DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_goal_position[1] = DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index]));
param_goal_position[2] = DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index]));
param_goal_position[3] = DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]));
// Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = groupSyncWrite.addParam(DXL1_ID, param_goal_position);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed", DXL1_ID);
return 0;
}
// Add Dynamixel#2 goal position value to the Syncwrite parameter storage
dxl_addparam_result = groupSyncWrite.addParam(DXL2_ID, param_goal_position);
if (dxl_addparam_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed", DXL2_ID);
return 0;
}
// Syncwrite goal position
dxl_comm_result = groupSyncWrite.txPacket();
if (dxl_comm_result != COMM_SUCCESS) packetHandler->printTxRxResult(dxl_comm_result);
// Clear syncwrite parameter storage
groupSyncWrite.clearParam();
do
{
// Syncread present position
dxl_comm_result = groupSyncRead.txRxPacket();
if (dxl_comm_result != COMM_SUCCESS) packetHandler->printTxRxResult(dxl_comm_result);
// Check if groupsyncread data of Dynamixel#1 is available
dxl_getdata_result = groupSyncRead.isAvailable(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
if (dxl_getdata_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL1_ID);
return 0;
}
// Check if groupsyncread data of Dynamixel#2 is available
dxl_getdata_result = groupSyncRead.isAvailable(DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
if (dxl_getdata_result != true)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL2_ID);
return 0;
}
// Get Dynamixel#1 present position value
dxl1_present_position = groupSyncRead.getData(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
// Get Dynamixel#2 present position value
dxl2_present_position = groupSyncRead.getData(DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position);
}while((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD));
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel#1 Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->printTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
// Disable Dynamixel#2 Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->printTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
// Close port
portHandler->closePort();
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;
}
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 Structs
int groupwrite_num = groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION);
// Initialize Groupsyncread Structs for Present Position
int groupread_num = groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
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
int32_t dxl1_present_position = 0, dxl2_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#1 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_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);
}
else
{
printf("Dynamixel#%d has been successfully connected \n", DXL1_ID);
}
// Enable Dynamixel#2 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_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);
}
else
{
printf("Dynamixel#%d has been successfully connected \n", DXL2_ID);
}
// Add parameter storage for Dynamixel#1 present position value
dxl_addparam_result = groupSyncReadAddParam(groupread_num, DXL1_ID);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead addparam failed", DXL1_ID);
return 0;
}
// Add parameter storage for Dynamixel#2 present position value
dxl_addparam_result = groupSyncReadAddParam(groupread_num, DXL2_ID);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead 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;
// Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL1_ID, dxl_goal_position[index], LEN_PRO_GOAL_POSITION);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed", DXL1_ID);
return 0;
}
// Add Dynamixel#2 goal position value to the Syncwrite parameter storage
dxl_addparam_result = groupSyncWriteAddParam(groupwrite_num, DXL2_ID, dxl_goal_position[index], LEN_PRO_GOAL_POSITION);
if (dxl_addparam_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed", DXL2_ID);
return 0;
}
// Syncwrite goal position
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
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 Dynamixel#1 is available
dxl_getdata_result = groupSyncReadIsAvailable(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL1_ID);
return 0;
}
// Check if groupsyncread data of Dynamixel#2 is available
dxl_getdata_result = groupSyncReadIsAvailable(groupread_num, DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
if (dxl_getdata_result != True)
{
fprintf(stderr, "[ID:%03d] groupSyncRead getdata failed", DXL2_ID);
return 0;
}
// Get Dynamixel#1 present position value
dxl1_present_position = groupSyncReadGetData(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
// Get Dynamixel#2 present position value
dxl2_present_position = groupSyncReadGetData(groupread_num, DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION);
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\t[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position);
} while ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (abs(dxl_goal_position[index] - dxl2_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_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);
}
// Disable Dynamixel#2 Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_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;
}