Dxl niryo_one_debug adding options get-register, factory-reset

niryo_one_debug/include/niryo_one_debug/dxl_tools.h

@@ -38,6 +38,8 @@ 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 factoryReset(int id);
 
         void closePort();
 

niryo_one_debug/src/dxl_debug.cpp

@@ -53,12 +53,19 @@ int main (int argc, char **argv)
             ("id,i", po::value<int>()->default_value(0), "Dxl motor ID")
             ("scan", "Scan all Dxl motors on the bus")
             ("ping", "ping specific ID")
-            ("set-register", po::value<std::vector<int>>(), "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[, ...])")
+            ("factory-reset", "Reset the motor to factory settings (id -> 1, baudrate -> 57600)");
+
+        po::options_description parserOptions("Options");
+        parserOptions.add(description);
+        parserOptions.add_options()
+            ("args", po::value<std::vector<int>>(), "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 +106,7 @@ int main (int argc, char **argv)
             }
         }
         else if (vars.count("set-register")) {
-            std::vector<int> params = vars["set-register"].as<std::vector<int>>();
+            std::vector<int> params = vars["args"].as<std::vector<int>>();
             if (params.size() != 3) {
                 printf("ERROR: set-register needs 3 arguments (reg_addr, value, size)\n");
             }
@@ -110,6 +117,26 @@ 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<int> params = vars["args"].as<std::vector<int>>();
+            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);
+                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 if (vars.count("factory-reset")) {
+            printf("--> FACTORY RESET for Motor (ID:%d)\n", id);
+            dxlTools.factoryReset(id);
+        }
         else {
             std::cout << description << "\n";
         }

niryo_one_debug/src/dxl_tools.cpp

@@ -108,6 +108,57 @@ 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;
+
+    uint8_t error = 0;
+
+    if (size == 1) {
+        dxl_comm_result = packetHandler->read1ByteTxRx(portHandler, (uint8_t) id,
+                (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, &error);
+    }
+    else if (size == 4) {
+        dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, (uint8_t) id,
+                (uint32_t)reg_address, (uint32_t*)&value, &error);
+    }
+    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: result %d, error %d\n", dxl_comm_result, 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();