From 88b67c847879632656f477044304e57dac4e23fb Mon Sep 17 00:00:00 2001 From: jon_s Date: Wed, 30 Oct 2019 14:32:08 +0900 Subject: [PATCH 1/4] Add get-register to Dxl debug tool --- .../include/niryo_one_debug/dxl_tools.h | 1 + niryo_one_debug/src/dxl_debug.cpp | 26 +++++++++++++--- niryo_one_debug/src/dxl_tools.cpp | 30 +++++++++++++++++++ 3 files changed, 53 insertions(+), 4 deletions(-) diff --git a/niryo_one_debug/include/niryo_one_debug/dxl_tools.h b/niryo_one_debug/include/niryo_one_debug/dxl_tools.h index c93036ee..0254b6da 100644 --- a/niryo_one_debug/include/niryo_one_debug/dxl_tools.h +++ b/niryo_one_debug/include/niryo_one_debug/dxl_tools.h @@ -38,6 +38,7 @@ class DxlTools { void broadcastPing(); void ping(int id); void setRegister(int id, int reg_address, int value, int size); + void getRegister(int id, int reg_address, int size); void closePort(); diff --git a/niryo_one_debug/src/dxl_debug.cpp b/niryo_one_debug/src/dxl_debug.cpp index 20b73f45..bc6e264d 100644 --- a/niryo_one_debug/src/dxl_debug.cpp +++ b/niryo_one_debug/src/dxl_debug.cpp @@ -53,12 +53,18 @@ int main (int argc, char **argv) ("id,i", po::value()->default_value(0), "Dxl motor ID") ("scan", "Scan all Dxl motors on the bus") ("ping", "ping specific ID") - ("set-register", po::value>(), "Set a value to a register (args: reg_addr, value, size)"); + ("set-register", "Set a value to a register (args: reg_addr, value, size)") + ("get-register", "Get the value of a register (args: reg_addr, size)"); + + po::options_description parserOptions("Options"); + parserOptions.add(description); + parserOptions.add_options() + ("args", po::value>(), "reg_addr, [value], size"); po::positional_options_description p; - p.add("set-register", -1); + p.add("args", -1); po::variables_map vars; - po::store(po::command_line_parser(argc, argv).options(description).positional(p).run(), vars); + po::store(po::command_line_parser(argc, argv).options(parserOptions).positional(p).run(), vars); po::notify(vars); // Display usage if no args or --help @@ -99,7 +105,7 @@ int main (int argc, char **argv) } } else if (vars.count("set-register")) { - std::vector params = vars["set-register"].as>(); + std::vector params = vars["args"].as>(); if (params.size() != 3) { printf("ERROR: set-register needs 3 arguments (reg_addr, value, size)\n"); } @@ -110,6 +116,18 @@ int main (int argc, char **argv) dxlTools.setRegister(id, params.at(0), params.at(1), params.at(2)); } } + else if (vars.count("get-register")) { + std::vector params = vars["args"].as>(); + if (params.size() != 2) { + printf("ERROR: get-register needs 2 arguments (reg_addr, size)\n"); + } + else { + printf("--> GET REGISTER for Motor (ID:%d)\n", id); + printf("Register address: %d, Size (bytes): %d\n", + params.at(0), params.at(1)); + dxlTools.getRegister(id, params.at(0), params.at(1)); + } + } else { std::cout << description << "\n"; } diff --git a/niryo_one_debug/src/dxl_tools.cpp b/niryo_one_debug/src/dxl_tools.cpp index f658270b..4f352064 100644 --- a/niryo_one_debug/src/dxl_tools.cpp +++ b/niryo_one_debug/src/dxl_tools.cpp @@ -108,6 +108,36 @@ void DxlTools::setRegister(int id, int reg_address, int value, int size) } } +void DxlTools::getRegister(int id, int reg_address, int size) +{ + int dxl_comm_result = COMM_TX_FAIL; + unsigned int value = 0; + + if (size == 1) { + dxl_comm_result = packetHandler->read1ByteTxRx(portHandler, (uint8_t) id, + (uint32_t)reg_address, (uint8_t*)&value); + } + else if (size == 2) { + dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, (uint8_t) id, + (uint32_t)reg_address, (uint16_t*)&value); + } + else if (size == 4) { + dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, (uint8_t) id, + (uint32_t)reg_address, (uint32_t*)&value); + } + else { + printf("ERROR: Size param must be 1, 2 or 4 bytes\n"); + return; + } + + if (dxl_comm_result != COMM_SUCCESS) { + printf("Failed to get register: %d\n", dxl_comm_result); + } + else { + printf("Register value = %d\n", value); + } +} + void DxlTools::closePort() { portHandler->closePort(); From 1f1194636e5125437bb26ca36cfef3e276076b1a Mon Sep 17 00:00:00 2001 From: jon_s Date: Wed, 30 Oct 2019 16:41:38 +0900 Subject: [PATCH 2/4] Modify Dxl debug tool get-register to return multiple values --- niryo_one_debug/src/dxl_debug.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/niryo_one_debug/src/dxl_debug.cpp b/niryo_one_debug/src/dxl_debug.cpp index bc6e264d..0603a82f 100644 --- a/niryo_one_debug/src/dxl_debug.cpp +++ b/niryo_one_debug/src/dxl_debug.cpp @@ -54,7 +54,7 @@ int main (int argc, char **argv) ("scan", "Scan all Dxl motors on the bus") ("ping", "ping specific ID") ("set-register", "Set a value to a register (args: reg_addr, value, size)") - ("get-register", "Get the value of a register (args: reg_addr, size)"); + ("get-register", "Get the value of a register (args: reg_addr, size[, ...])"); po::options_description parserOptions("Options"); parserOptions.add(description); @@ -118,14 +118,18 @@ int main (int argc, char **argv) } else if (vars.count("get-register")) { std::vector params = vars["args"].as>(); - if (params.size() != 2) { - printf("ERROR: get-register needs 2 arguments (reg_addr, size)\n"); + if (params.size() < 2) { + printf("ERROR: get-register needs 2 or more arguments (reg_addr, size[, ...])\n"); } else { printf("--> GET REGISTER for Motor (ID:%d)\n", id); - printf("Register address: %d, Size (bytes): %d\n", - params.at(0), params.at(1)); - dxlTools.getRegister(id, params.at(0), params.at(1)); + int reg_address = params.at(0); + for (int idx = 1; idx < params.size(); idx++) { + printf("Register address: %d, Size (bytes): %d\n", + reg_address, params.at(idx)); + dxlTools.getRegister(id, reg_address, params.at(idx)); + reg_address += params.at(idx); + } } } else { From 5c73f67ab69abda944c6b56cd5a39d863829d610 Mon Sep 17 00:00:00 2001 From: jon_s Date: Wed, 30 Oct 2019 19:03:14 +0900 Subject: [PATCH 3/4] Modify Dxl debug tool get-register for more error info --- niryo_one_debug/src/dxl_tools.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/niryo_one_debug/src/dxl_tools.cpp b/niryo_one_debug/src/dxl_tools.cpp index 4f352064..0bf5292a 100644 --- a/niryo_one_debug/src/dxl_tools.cpp +++ b/niryo_one_debug/src/dxl_tools.cpp @@ -113,17 +113,19 @@ void DxlTools::getRegister(int id, int reg_address, int size) int dxl_comm_result = COMM_TX_FAIL; unsigned int value = 0; + uint8_t error = 0; + if (size == 1) { dxl_comm_result = packetHandler->read1ByteTxRx(portHandler, (uint8_t) id, - (uint32_t)reg_address, (uint8_t*)&value); + (uint32_t)reg_address, (uint8_t*)&value, &error); } else if (size == 2) { dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, (uint8_t) id, - (uint32_t)reg_address, (uint16_t*)&value); + (uint32_t)reg_address, (uint16_t*)&value, &error); } else if (size == 4) { dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, (uint8_t) id, - (uint32_t)reg_address, (uint32_t*)&value); + (uint32_t)reg_address, (uint32_t*)&value, &error); } else { printf("ERROR: Size param must be 1, 2 or 4 bytes\n"); @@ -131,7 +133,9 @@ void DxlTools::getRegister(int id, int reg_address, int size) } if (dxl_comm_result != COMM_SUCCESS) { - printf("Failed to get register: %d\n", dxl_comm_result); + printf("Failed to get register: result %d, error %d\n", dxl_comm_result, error); + packetHandler.printTxRxResult(dxl_comm_result); + packetHandler.printRxPacketError(error); } else { printf("Register value = %d\n", value); From cd570783281c03b5a7d645f7d07efad0c039c062 Mon Sep 17 00:00:00 2001 From: jon_s Date: Wed, 30 Oct 2019 19:53:23 +0900 Subject: [PATCH 4/4] Add factory-reset to Dxl debug tool --- .../include/niryo_one_debug/dxl_tools.h | 1 + niryo_one_debug/src/dxl_debug.cpp | 7 ++++++- niryo_one_debug/src/dxl_tools.cpp | 21 +++++++++++++++++-- 3 files changed, 26 insertions(+), 3 deletions(-) diff --git a/niryo_one_debug/include/niryo_one_debug/dxl_tools.h b/niryo_one_debug/include/niryo_one_debug/dxl_tools.h index 0254b6da..6342f247 100644 --- a/niryo_one_debug/include/niryo_one_debug/dxl_tools.h +++ b/niryo_one_debug/include/niryo_one_debug/dxl_tools.h @@ -39,6 +39,7 @@ class DxlTools { void ping(int id); void setRegister(int id, int reg_address, int value, int size); void getRegister(int id, int reg_address, int size); + void factoryReset(int id); void closePort(); diff --git a/niryo_one_debug/src/dxl_debug.cpp b/niryo_one_debug/src/dxl_debug.cpp index 0603a82f..0b96f4b2 100644 --- a/niryo_one_debug/src/dxl_debug.cpp +++ b/niryo_one_debug/src/dxl_debug.cpp @@ -54,7 +54,8 @@ int main (int argc, char **argv) ("scan", "Scan all Dxl motors on the bus") ("ping", "ping specific ID") ("set-register", "Set a value to a register (args: reg_addr, value, size)") - ("get-register", "Get the value of a register (args: reg_addr, size[, ...])"); + ("get-register", "Get the value of a register (args: reg_addr, size[, ...])") + ("factory-reset", "Reset the motor to factory settings (id -> 1, baudrate -> 57600)"); po::options_description parserOptions("Options"); parserOptions.add(description); @@ -132,6 +133,10 @@ int main (int argc, char **argv) } } } + else if (vars.count("factory-reset")) { + printf("--> FACTORY RESET for Motor (ID:%d)\n", id); + dxlTools.factoryReset(id); + } else { std::cout << description << "\n"; } diff --git a/niryo_one_debug/src/dxl_tools.cpp b/niryo_one_debug/src/dxl_tools.cpp index 0bf5292a..c248cc42 100644 --- a/niryo_one_debug/src/dxl_tools.cpp +++ b/niryo_one_debug/src/dxl_tools.cpp @@ -134,14 +134,31 @@ void DxlTools::getRegister(int id, int reg_address, int size) if (dxl_comm_result != COMM_SUCCESS) { printf("Failed to get register: result %d, error %d\n", dxl_comm_result, error); - packetHandler.printTxRxResult(dxl_comm_result); - packetHandler.printRxPacketError(error); + packetHandler->printTxRxResult(dxl_comm_result); + packetHandler->printRxPacketError(error); } else { printf("Register value = %d\n", value); } } +void DxlTools::factoryReset(int id) +{ + int dxl_comm_result = COMM_TX_FAIL; + uint8_t error = 0; + + dxl_comm_result = packetHandler->factoryReset(portHandler, (uint8_t) id, (uint8_t) 0xFF, &error); + + if (dxl_comm_result != COMM_SUCCESS) { + printf("Factory reset failed: result %d, error %d\n", dxl_comm_result, error); + packetHandler->printTxRxResult(dxl_comm_result); + packetHandler->printRxPacketError(error); + } + else { + printf("Factory reset successful (id and baudrate reset)\n"); + } +} + void DxlTools::closePort() { portHandler->closePort();