Improved ping and added RW for CLI manipulation

Author: vytasrgl

.gitignore

@@ -2,3 +2,6 @@
 __pycache__
 .venv
 .vscode
+build
+dist
+*.spec

README.md

@@ -28,6 +28,9 @@ Pings all available servos on multiple boards connected
   --from_id FROM_ID     From ID (default: 0)
   --to_id TO_ID         To ID (default: 110)
   --json JSON           Output as JSON (default: False)
+  --find_any FIND_ANY   If True, will print motor ID and return immediatly
+                        after found, if none servo found it will fail with
+                        exit code 1 (default: False)
 ```
 Examples:
 ```
@@ -40,6 +43,53 @@ Output:
 Where device port is `/dev/ttyUSB0` and `83` is servo ID and `6662` internal servo model.
 
 
+### RW
+Allows to read and write registers to the servo
+```bash
+./rw.py --help
+```
+
+```
+usage: rw.py [-h] [--hardware_regex HARDWARE_REGEX] [--baud BAUD]
+             [--protocol PROTOCOL] --id ID [--addr ADDR] [--length {1,2}]
+             [--negative_bit NEGATIVE_BIT] [--write WRITE]
+             [--action {reset_neutral,id}]
+
+Read Write Feetech registers
+
+optional arguments:
+  -h, --help            show this help message and exit
+  --hardware_regex HARDWARE_REGEX
+                        Serial port filter for detecting multiple boards or
+                        specify single board (default: 1A86:7523)
+  --baud BAUD           Baudrate (default: 1000000)
+  --protocol PROTOCOL   SCS Protocol (default: 0)
+  --id ID               Servo ID (default: None)
+  --addr ADDR           Register (default: None)
+  --length {1,2}        Address length in bytes (default: None)
+  --negative_bit NEGATIVE_BIT
+                        Negative sign bit for this register (default: None)
+  --write WRITE         Data to write in the decimal format (default: None)
+  --action {reset_neutral,id}
+                        Defined actions such as set (default: None)
+```
+Examples:
+Writes goal_position:
+```
+ ./rw.py --id 3 --addr 42 --length 2 --write 1000
+```
+
+Resets servo to neutral:
+```
+./rw.py --id 3 --action  reset_neutral
+```
+
+Reads present position:
+```
+./rw.py --id 3 --addr 56 --length 2
+```
+
+
 ### Meassure
 Measures the time it takes for servo to move from one position to another
 Usage:

ping.py

@@ -28,13 +28,16 @@
 def argument_parser():
     parser = argparse.ArgumentParser(description="Ping all feetech servos", formatter_class=argparse.ArgumentDefaultsHelpFormatter)
     # By default we search for all CH430 ports with
-    parser.add_argument("--hardware_regex", type=str, default='1A86:7523', help='Serial port filter for detecting multiple boards')
+    parser.add_argument("--hardware_regex", type=str, default='1A86:7523', help='Serial port filter for detecting multiple boards or specify single board')
     parser.add_argument("--baud", type=int, default=1000000, help='Baudrate')
     parser.add_argument("--from_id", type=int, default=0, help='From ID')
     parser.add_argument("--to_id", type=int, default=110, help='To ID')
     parser.add_argument("--json", type=bool, default=False, help="Output as JSON")
+    parser.add_argument("--find_any", type=bool, default=False, help="If True, will print motor ID and return immediatly after found, if none servo found it will fail with exit code 1")
     args = parser.parse_args()
     return vars(args)
+# Global dictionary for args
+args = argument_parser()
 
 def pingservos(port, baud, from_id, to_id):
     port = sdk.PortHandler(port)
@@ -49,22 +52,31 @@ def pingservos(port, baud, from_id, to_id):
             model_number, result, error = handler.ping(port, id)
             if result == sdk.COMM_SUCCESS:
                 servos.append((id, model_number))
+                if args['find_any']:
+                    servo_found(id)
         return servos
     except Exception as e:
         print(e)
         return None
 
-def find_all_servos(args):
+def find_all_servos():
     ports = [p.device for p in grep_serial_ports(args['hardware_regex'])]
     devices = {}
     for p in ports:
         devices[p] = pingservos(p, args['baud'], args['from_id'], args['to_id'])
     return devices
 
+def servo_found(id):
+    # Simple output
+    print(id)
+    exit(0)
 
 def main():
-    args = argument_parser()
-    device_servos = find_all_servos(args)
+    device_servos = find_all_servos()
+    if args['find_any']:
+        print("No servo found")
+        exit(1)
+
     if args['json']:
         print(json.dumps(device_servos, indent=4, sort_keys=True))
     else:

rw.py

@@ -0,0 +1,132 @@
+#!/usr/bin/env python
+#
+# Copyright (c) 2022 Hanson Robotics.
+#
+# This file is part of Hanson AI.
+# See https://www.hansonrobotics.com/hanson-ai for further info.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+import argparse
+import json
+from pprint import pprint as pp
+from serial.tools.list_ports import grep as grep_serial_ports
+import scservo_sdk as sdk
+
+ACTIONS = {
+    'reset_neutral':
+        {
+            'addr':     40,
+            'length':   1,
+            'write':    128
+        },
+    'id':
+        {
+            'addr':     5,
+            'length':   1,
+        }
+}
+
+def argument_parser():
+    parser = argparse.ArgumentParser(description="Read Write Feetech registers", formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    # By default we search for all CH430 ports with
+    parser.add_argument("--hardware_regex", type=str, default='1A86:7523', help='Serial port filter for detecting multiple boards or specify single board')
+    parser.add_argument("--baud", type=int, default=1000000, help='Baudrate')
+    parser.add_argument("--protocol", type=int, default=0, help='SCS Protocol')
+    parser.add_argument("--id", type=int, help='Servo ID', required=True)
+    parser.add_argument("--addr", type=int, help='Register')
+    parser.add_argument("--length", type=int, help='Address length in bytes', choices=[1,2])
+    parser.add_argument("--negative_bit", type=int, help='Negative sign  bit for this register')
+    parser.add_argument("--write", type=int, help='Data to write in the decimal format ')
+    parser.add_argument("--action", type=str, help="Defined actions such as set", choices=list(ACTIONS.keys()))
+    args = parser.parse_args()
+    args = vars(args)
+    if args.get('action', False):
+        args.update(ACTIONS[args['action']])
+    if not args.get('addr', False):
+        parser.error("Register address is required")
+    if not args.get('length', False):
+        parser.error("Register address length is required")
+    return args
+
+
+args = argument_parser()
+
+def find_servo_port(port, baud,  id):
+    port = sdk.PortHandler(port)
+    handler = sdk.PacketHandler(args['protocol'])
+    try:
+        port.openPort()
+        port.setBaudRate(baud)
+        model_number, result, error = handler.ping(port, id)
+        if result == sdk.COMM_SUCCESS:
+            return port, handler
+    except Exception as e:
+        print(e)
+    return None, None
+
+def find_servo():
+    ports = [p.device for p in grep_serial_ports(args['hardware_regex'])]
+    for p in ports:
+        port, handler =  find_servo_port(p, args['baud'], args['id'])
+        if port is not None:
+            return port, handler
+    print("Servo not found")
+    exit(1)
+
+def write(port, handler):
+    data = args['write']
+    length = args['length']
+    # negative values
+    if data < 0:
+        bit  = args['negative_bit']
+        if bit < 2 or bit > length * 8 - 1:
+            print("Invalid negative bit")
+        data = abs(data) + 2**bit
+    if length == 1:
+        result, error = handler.write1ByteTxRx(port, args['id'], args['addr'], data)
+    if length == 2:
+        result, error = handler.write2ByteTxRx(port, args['id'], args['addr'], data)
+    if not result == sdk.COMM_SUCCESS:
+        print("Error wiriting data")
+        exit(1)
+    else:
+        print("Sucessfully written")
+        exit()
+
+def read(port, handler):
+    length = args['length']
+    if length == 1:
+        data, result, error = handler.read1ByteTxRx(port, args['id'], args['addr'])
+    if length == 2:
+        data, result, error = handler.read2ByteTxRx(port, args['id'], args['addr'])
+    if not result == sdk.COMM_SUCCESS:
+        print("Error reading data")
+        exit(1)
+    else:
+        if args['negative_bit'] is not None:
+            bit = args['negative_bit']
+            if data > 2**bit:
+                data = 0-data%(2**bit)
+        print(data)
+
+def main():
+    port, handler = find_servo()
+    if args.get('write') is not None:
+        write(port, handler)
+    else:
+        read(port, handler)
+
+if __name__ == '__main__':
+    main()