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