diff --git a/dynamixel_workbench_toolbox/examples/src/b_Ping.cpp b/dynamixel_workbench_toolbox/examples/src/b_Ping.cpp index 3419ed7..cae3d2e 100644 --- a/dynamixel_workbench_toolbox/examples/src/b_Ping.cpp +++ b/dynamixel_workbench_toolbox/examples/src/b_Ping.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -52,7 +53,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeeded to init(%d)\n", baud_rate); + printf("Succeeded to init(%d)\n", baud_rate); result = dxl_wb.ping(dxl_id, &model_number, &log); if (result == false) @@ -67,4 +68,4 @@ int main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/c_ID_Change.cpp b/dynamixel_workbench_toolbox/examples/src/c_ID_Change.cpp index 9bccf42..780d003 100644 --- a/dynamixel_workbench_toolbox/examples/src/c_ID_Change.cpp +++ b/dynamixel_workbench_toolbox/examples/src/c_ID_Change.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -52,7 +53,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeeded to init(%d)\n", baud_rate); + printf("Succeeded to init(%d)\n", baud_rate); uint16_t model_number = 0; result = dxl_wb.ping(dxl_id, &model_number, &log); @@ -98,4 +99,4 @@ int main(int argc, char *argv[]) for (int cnt = 0; cnt < dxl_cnt; cnt++) printf("id : %d, model name : %s\n", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt])); } -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/d_BPS_Change.cpp b/dynamixel_workbench_toolbox/examples/src/d_BPS_Change.cpp index 46a91eb..b117319 100644 --- a/dynamixel_workbench_toolbox/examples/src/d_BPS_Change.cpp +++ b/dynamixel_workbench_toolbox/examples/src/d_BPS_Change.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int dxl_id = 1; @@ -52,7 +53,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeeded to init(%d)\n", baud_rate); + printf("Succeeded to init(%d)\n", baud_rate); uint16_t model_number = 0; result = dxl_wb.ping(dxl_id, &model_number, &log); @@ -81,4 +82,4 @@ int main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/e_Mode_Change.cpp b/dynamixel_workbench_toolbox/examples/src/e_Mode_Change.cpp index ea12c4b..b66c95b 100644 --- a/dynamixel_workbench_toolbox/examples/src/e_Mode_Change.cpp +++ b/dynamixel_workbench_toolbox/examples/src/e_Mode_Change.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -53,7 +54,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeeded to init(%d)\n", baud_rate); + printf("Succeeded to init(%d)\n", baud_rate); result = dxl_wb.ping(dxl_id, &model_number, &log); if (result == false) diff --git a/dynamixel_workbench_toolbox/examples/src/f_Reboot.cpp b/dynamixel_workbench_toolbox/examples/src/f_Reboot.cpp index 6521b17..19dc564 100644 --- a/dynamixel_workbench_toolbox/examples/src/f_Reboot.cpp +++ b/dynamixel_workbench_toolbox/examples/src/f_Reboot.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -52,7 +53,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeed to init(%d)\n", baud_rate); + printf("Succeed to init(%d)\n", baud_rate); result = dxl_wb.ping(dxl_id, &model_number, &log); if (result == false) @@ -78,4 +79,4 @@ int main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/g_Reset.cpp b/dynamixel_workbench_toolbox/examples/src/g_Reset.cpp index 26c2258..d95edb0 100644 --- a/dynamixel_workbench_toolbox/examples/src/g_Reset.cpp +++ b/dynamixel_workbench_toolbox/examples/src/g_Reset.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -52,7 +53,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeed to init(%d)\n", baud_rate); + printf("Succeed to init(%d)\n", baud_rate); result = dxl_wb.ping(dxl_id, &model_number, &log); if (result == false) @@ -78,4 +79,4 @@ int main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/h_Position.cpp b/dynamixel_workbench_toolbox/examples/src/h_Position.cpp index b47a9f4..a6d8a38 100644 --- a/dynamixel_workbench_toolbox/examples/src/h_Position.cpp +++ b/dynamixel_workbench_toolbox/examples/src/h_Position.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -52,7 +53,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeed to init(%d)\n", baud_rate); + printf("Succeed to init(%d)\n", baud_rate); result = dxl_wb.ping(dxl_id, &model_number, &log); if (result == false) @@ -88,4 +89,4 @@ int main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/i_Velocity.cpp b/dynamixel_workbench_toolbox/examples/src/i_Velocity.cpp index 25cb945..62c002a 100644 --- a/dynamixel_workbench_toolbox/examples/src/i_Velocity.cpp +++ b/dynamixel_workbench_toolbox/examples/src/i_Velocity.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -52,7 +53,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeed to init(%d)\n", baud_rate); + printf("Succeed to init(%d)\n", baud_rate); result = dxl_wb.ping(dxl_id, &model_number, &log); if (result == false) @@ -90,4 +91,4 @@ int main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/j_Current_Based_Position.cpp b/dynamixel_workbench_toolbox/examples/src/j_Current_Based_Position.cpp index f078385..9eac8d5 100644 --- a/dynamixel_workbench_toolbox/examples/src/j_Current_Based_Position.cpp +++ b/dynamixel_workbench_toolbox/examples/src/j_Current_Based_Position.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -52,7 +53,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeed to init(%d)\n", baud_rate); + printf("Succeed to init(%d)\n", baud_rate); result = dxl_wb.ping(dxl_id, &model_number, &log); if (result == false) @@ -88,4 +89,4 @@ int main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/k_Read_Write.cpp b/dynamixel_workbench_toolbox/examples/src/k_Read_Write.cpp index ad6045d..4990e8f 100644 --- a/dynamixel_workbench_toolbox/examples/src/k_Read_Write.cpp +++ b/dynamixel_workbench_toolbox/examples/src/k_Read_Write.cpp @@ -17,8 +17,9 @@ /* Authors: Taehun Lim (Darby) */ #include +#include -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -52,7 +53,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeed to init(%d)\n", baud_rate); + printf("Succeed to init(%d)\n", baud_rate); result = dxl_wb.ping(dxl_id, &model_number, &log); if (result == false) @@ -90,4 +91,4 @@ int main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/l_Sync_Write.cpp b/dynamixel_workbench_toolbox/examples/src/l_Sync_Write.cpp index fe5b381..e6300f9 100644 --- a/dynamixel_workbench_toolbox/examples/src/l_Sync_Write.cpp +++ b/dynamixel_workbench_toolbox/examples/src/l_Sync_Write.cpp @@ -17,10 +17,11 @@ /* Authors: Taehun Lim (Darby) */ #include +#include void swap(int32_t *array); -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -55,7 +56,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeed to init(%d)\n", baud_rate); + printf("Succeed to init(%d)\n", baud_rate); for (int cnt = 0; cnt < 2; cnt++) { @@ -93,7 +94,7 @@ int main(int argc, char *argv[]) int32_t goal_position[2] = {0, 1023}; const uint8_t handler_index = 0; - + while(1) { result = dxl_wb.syncWrite(handler_index, &goal_position[0], &log); @@ -116,4 +117,4 @@ void swap(int32_t *array) int32_t tmp = array[0]; array[0] = array[1]; array[1] = tmp; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/m_Sync_Read_Write.cpp b/dynamixel_workbench_toolbox/examples/src/m_Sync_Read_Write.cpp index 7c24729..df3045b 100644 --- a/dynamixel_workbench_toolbox/examples/src/m_Sync_Read_Write.cpp +++ b/dynamixel_workbench_toolbox/examples/src/m_Sync_Read_Write.cpp @@ -17,10 +17,11 @@ /* Authors: Taehun Lim (Darby) */ #include +#include void swap(int32_t *array); -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -55,7 +56,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeed to init(%d)\n", baud_rate); + printf("Succeed to init(%d)\n", baud_rate); for (int cnt = 0; cnt < 2; cnt++) { @@ -69,6 +70,7 @@ int main(int argc, char *argv[]) { printf("Succeeded to ping\n"); printf("id : %d, model_number : %d\n", dxl_id[cnt], model_number); + printf("model_name : %s\n", dxl_wb.getModelName(dxl_id[cnt])); } result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log); @@ -101,7 +103,10 @@ int main(int argc, char *argv[]) int32_t present_position[2] = {0, 0}; const uint8_t handler_index = 0; - + + const char* modelName1 = dxl_wb.getModelName(dxl_id[0]); + const char* modelName2 = dxl_wb.getModelName(dxl_id[1]); + while(1) { result = dxl_wb.syncWrite(handler_index, &goal_position[0], &log); @@ -124,6 +129,12 @@ int main(int argc, char *argv[]) if (result == false) { printf("%s\n", log); + if(!strncmp(modelName1, "AX-12", strlen("AX-12")) || + !strncmp(modelName2, "AX-12", strlen("AX-12"))) + { + printf("AX-12 does NOT support syncRead \n"); + return -1; + } } else { @@ -131,7 +142,7 @@ int main(int argc, char *argv[]) ,dxl_id[0], goal_position[0], present_position[0], dxl_id[1], goal_position[1], present_position[1]); } - }while(abs(goal_position[0] - present_position[0]) > 15 && + }while(abs(goal_position[0] - present_position[0]) > 15 && abs(goal_position[1] - present_position[1]) > 15); swap(goal_position); @@ -145,4 +156,4 @@ void swap(int32_t *array) int32_t tmp = array[0]; array[0] = array[1]; array[1] = tmp; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/n_Bulk_Read_Write.cpp b/dynamixel_workbench_toolbox/examples/src/n_Bulk_Read_Write.cpp index 6fc2e07..ee10783 100644 --- a/dynamixel_workbench_toolbox/examples/src/n_Bulk_Read_Write.cpp +++ b/dynamixel_workbench_toolbox/examples/src/n_Bulk_Read_Write.cpp @@ -17,10 +17,11 @@ /* Authors: Taehun Lim (Darby) */ #include +#include void swap(int32_t *array); -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; int baud_rate = 57600; @@ -55,7 +56,7 @@ int main(int argc, char *argv[]) return 0; } else - printf("Succeed to init(%d)\n", baud_rate); + printf("Succeed to init(%d)\n", baud_rate); for (int cnt = 0; cnt < 2; cnt++) { @@ -197,4 +198,4 @@ void swap(int32_t *array) int32_t tmp = array[0]; array[0] = array[1]; array[1] = tmp; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/o_Find_Dynamixel.cpp b/dynamixel_workbench_toolbox/examples/src/o_Find_Dynamixel.cpp index 5615952..69ccfa4 100644 --- a/dynamixel_workbench_toolbox/examples/src/o_Find_Dynamixel.cpp +++ b/dynamixel_workbench_toolbox/examples/src/o_Find_Dynamixel.cpp @@ -17,10 +17,11 @@ /* Authors: Taehun Lim (Darby) */ #include +#include #define BAUDRATE_NUM 7 -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; @@ -56,7 +57,7 @@ int main(int argc, char *argv[]) printf("Failed to init\n"); } else - printf("Succeed to init(%d)\n", baudrate[index]); + printf("Succeed to init(%d)\n", baudrate[index]); dxl_cnt = 0; for (uint8_t num = 0; num < 100; num++) scanned_id[num] = 0; @@ -80,4 +81,4 @@ int main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/dynamixel_workbench_toolbox/examples/src/p_Monitor.cpp b/dynamixel_workbench_toolbox/examples/src/p_Monitor.cpp index 05b2082..1bf36f0 100644 --- a/dynamixel_workbench_toolbox/examples/src/p_Monitor.cpp +++ b/dynamixel_workbench_toolbox/examples/src/p_Monitor.cpp @@ -17,6 +17,7 @@ /* Authors: Taehun Lim (Darby) */ #include +#include #include // FILE control #include // Terminal IO #include @@ -39,7 +40,7 @@ bool monitoring(const char* port_name); DynamixelWorkbench dxl_wb; -int main(int argc, char *argv[]) +int main(int argc, char *argv[]) { const char* port_name = "/dev/ttyUSB0"; @@ -109,7 +110,7 @@ bool monitoring(const char* port_name) else if (strcmp(cmd, "begin") == 0) { uint32_t baud = 57600; - + baud = atoi(param[0]); wb_result = dxl_wb.init(port_name, baud, &log); if (wb_result == false) @@ -118,16 +119,16 @@ bool monitoring(const char* port_name) printf("Failed to init\n"); } else - printf("Succeed to init(%d)\n", baud); + printf("Succeed to init(%d)\n", baud); } else if (strcmp(cmd, "end") == 0) - { + { exit(0); } else if (strcmp(cmd, "scan") == 0) { uint8_t range = 253; // default - + range = atoi(param[0]); wb_result = dxl_wb.scan(get_id, &scan_cnt, range, &log); if (wb_result == false) @@ -146,7 +147,7 @@ bool monitoring(const char* port_name) else if (strcmp(cmd, "ping") == 0) { get_id[ping_cnt] = 1; // default to 1 - + get_id[ping_cnt] = atoi(param[0]); uint16_t model_number = 0; @@ -219,7 +220,7 @@ bool monitoring(const char* port_name) default: data = getAllRegisteredData[control_item[index].address]; break; - } + } } } } @@ -353,7 +354,7 @@ bool monitoring(const char* port_name) uint8_t id_1 = atoi(param[0]); uint8_t id_2 = atoi(param[1]); uint8_t id[2] = {id_1, id_2}; - uint8_t id_num = 2; + uint8_t id_num = 2; int32_t data[2] = {0, 0}; data[0] = atoi(param[3]); @@ -543,7 +544,7 @@ bool monitoring(const char* port_name) uint8_t id = atoi(param[0]); int32_t data = 0; - + wb_result = dxl_wb.readRegister(id, param[1], &data, &log); if (wb_result == false) {