Merge pull request #53 from iory/armh7

Enable armh tools

README.md

@@ -1 +1,93 @@
 # rcb4
+
+## For JSK Users
+
+### Worm Gear Module Calibration Tool
+
+`armh7-tools` provides a comprehensive solution for calibrating and managing worm gear modules connected via the `ARMH7Interface`.
+It facilitates the calibration of worm gears, reading of calibrated sensor data, and real-time display of sensor and worm gear values.
+Designed for flexibility, it supports operations like calibration data update and in-place modification of YAML configuration files.
+
+#### YAML Configuration Format
+
+The calibration tool requires an input YAML file for worm gear modules configuration. 
+This file contains a list of worm gears with their associated parameters: `worm_id`, `servo_id`, `sensor_id`,
+and initial magnetic encoder value `magenc_init`.
+Each worm gear is represented by a line in the YAML file with its parameters encapsulated in curly braces and prefixed by a dash, indicating a list item in YAML syntax.
+
+```
+ - {worm_id: 0, servo_id: 0, sensor_id: 38, magenc_init: 6502}
+ - {worm_id: 1, servo_id: 2, sensor_id: 40, magenc_init: 3039}
+ - {worm_id: 3, servo_id: 6, sensor_id: 44, magenc_init: 9502}
+```
+
+#### Reading Worm Gear Modules's calibration data
+
+```
+armh7-tools read-calib [--device DEVICE] output_file_path
+```
+
+`--device`, `-d`: Specify the device port. (Default: None)
+`output_file_path`: Path to output the YAML file with calibration data.
+
+
+##### Example
+
+```
+armh7-tools read-calib ./worm_calib.yaml
+```
+
+#### Calibrate Worm Gear Modules
+
+```
+armh7-tools calibrate [--device DEVICE] file_path [--inplace] [--update] [--output OUTPUT]
+```
+
+`--device`, `-d`: Specify the device port. (Default: None)
+`file_path`: Path to the input YAML file containing worm gear configurations.
+`--inplace`, `-i`: Overwrite the input YAML file with calibration results. (Optional)
+`--update`, `-u`: Ignore and overwrite the current magenc_init values in the input YAML. (Optional)
+`--output`, `-o`: Specify a path to save the calibrated data if not overwriting in-place. (Optional)
+
+##### Example
+
+When the `update` option is specified, the current posture of the robot will be modified, 
+and upon pressing the Enter key, the angle at that moment will be set as the zero point. 
+Consequently, the `magenc_init` value will be updated and written to the board, 
+
+```
+armh7-tools calibrate ./worm_calib.yaml --update
+```
+
+#### Print Sensor Values
+
+This command displays real-time sensor values in a table format.
+
+```
+armh7-tools print-sensor
++------|----------|--------------|--------------|--------------|--------------|----------|----------|----------|----------+
+|   ID |   Magenc |   Proximity1 |   Proximity2 |   Proximity3 |   Proximity4 |   Force1 |   Force2 |   Force3 |   Force4 |
+|------|----------|--------------|--------------|--------------|--------------|----------|----------|----------|----------|
+|   19 |     7365 |            0 |            0 |            0 |            0 |     1240 |     1167 |     1083 |     1037 |
+|   20 |     3042 |            0 |            0 |            0 |            0 |     1209 |     1106 |     1075 |     1025 |
+|   21 |    13800 |            0 |            0 |            0 |            0 |     1230 |     1106 |     1064 |     1003 |
+|   22 |     9512 |            0 |            0 |            0 |            0 |     1222 |     1144 |     1123 |     1085 |
+|   23 |    14182 |            0 |            0 |            0 |            0 |     1238 |     1179 |     1083 |     1040 |
++------|----------|--------------|--------------|--------------|--------------|----------|----------|----------|----------+
+```
+
+#### Print Worm Values
+
+Displays real-time worm gear values in a table format.
+
+```
+armh7-tools print-worm
++------------|-------------|---------------|------------------|-----------------+
+|   servo_id |   sensor_id |   magenc_init |   magenc_present |   present_angle |
+|------------|-------------|---------------|------------------|-----------------|
+|          0 |          38 |          6502 |             7368 |      -19.0283   |
+|          2 |          40 |          3039 |             3039 |       -0        |
+|          6 |          44 |          9502 |             9513 |       -0.241699 |
+|          8 |          46 |          9783 |            14170 |      -96.394    |
++------------|-------------|---------------|------------------|-----------------+
+```

rcb4/apps/armh7_tool.py

@@ -0,0 +1,161 @@
+#!/usr/bin/env python
+
+import argparse
+
+import pandas as pd
+from tabulate import tabulate
+import yaml
+
+from rcb4.armh7interface import ARMH7Interface
+
+
+def calibrate_worms(interface, file_path, output_path,
+                    update=False,
+                    inplace=False):
+    with open(file_path, 'r') as file:
+        data = yaml.safe_load(file)
+
+    output_str = ''
+    for entry in data:
+        print(entry)
+        worm_id = entry['worm_id']
+        servo_id = entry['servo_id']
+        sensor_id = entry['sensor_id']
+        magenc_present = entry.get('magenc_init', None)
+        if update is True:
+            magenc_present = None
+        calibrated_value = interface.calibrate_worm(
+            worm_id, servo_id, sensor_id, magenc_present)
+        entry['magenc_init'] = calibrated_value
+        output_str += f' - {{worm_id: {worm_id}, servo_id: {servo_id}, sensor_id: {sensor_id}, magenc_init: {calibrated_value}}}\n'  # NOQA
+    interface.databssram_to_dataflash()
+    if inplace:
+        output_path = file_path
+    print(output_str, end='')
+    if output_path:
+        with open(output_path, 'w') as file:
+            file.write(output_str)
+        print('Worm calibration file is saved to {}'.format(output_path))
+
+
+def read_calib_sensors(interface, output_path):
+    output_str = ''
+    for worm_id in interface.search_worm_ids():
+        worm = interface.read_worm_calib_data(worm_id)
+        output_str += f' - {{worm_id: {worm_id}, servo_id: {worm.servo_id}, sensor_id: {worm.sensor_id}, magenc_init: {worm.magenc_init}}}\n'  # NOQA
+    print(output_str, end='')
+    with open(output_path, 'w') as file:
+        file.write(output_str)
+    print('Worm calibration file is saved to {}'.format(output_path))
+
+
+def print_sensor_values(interface):
+    try:
+        while True:
+            sensors = interface.all_jointbase_sensors()
+            data = {
+                'ID': [sensor.id for sensor in sensors],
+                'Magenc': [sensor.magenc for sensor in sensors],
+                'Proximity1': [sensor.ps[0] for sensor in sensors],
+                'Proximity2': [sensor.ps[1] for sensor in sensors],
+                'Proximity3': [sensor.ps[2] for sensor in sensors],
+                'Proximity4': [sensor.ps[3] for sensor in sensors],
+                'Force1': [sensor.adc[0] for sensor in sensors],
+                'Force2': [sensor.adc[1] for sensor in sensors],
+                'Force3': [sensor.adc[2] for sensor in sensors],
+                'Force4': [sensor.adc[3] for sensor in sensors]
+            }
+            df = pd.DataFrame(data)
+
+            print(tabulate(df, headers='keys', tablefmt='psql',
+                           showindex=False))
+            print("\033[H\033[J", end="")
+    except KeyboardInterrupt:
+        pass
+
+
+def print_worm_values(interface):
+    attributes = [
+        'servo_id', 'sensor_id',
+        'magenc_init', 'magenc_present', 'present_angle',
+    ]
+    try:
+        while True:
+            sensors = [interface.read_worm_calib_data(worm_idx)
+                       for worm_idx in interface.search_worm_ids()]
+            data = {attr: [] for attr in attributes}
+            for sensor in sensors:
+                for attr in attributes:
+                    data[attr].append(getattr(sensor, attr, 'N/A'))
+            df = pd.DataFrame(data)
+            print(tabulate(df, headers='keys', tablefmt='psql',
+                           showindex=False))
+            print("\033[H\033[J", end="")
+    except KeyboardInterrupt:
+        pass
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    subparsers = parser.add_subparsers()
+
+    parser_calibrate = subparsers.add_parser(
+        'calibrate', help='Calibrate worm gear module.')
+    parser_calibrate.add_argument('--device', '-d', default=None,
+                                  help='Input device port.')
+    parser_calibrate.add_argument(
+        'file_path', type=str, help='Path to yaml file.')
+    parser_calibrate.add_argument(
+        '--inplace', '-i', action='store_true',
+        help='Overwrite file_path.')
+    parser_calibrate.add_argument(
+        '--update', '-u', action='store_true',
+        help="Overwrite input yaml's current magenc_init.")
+    parser_calibrate.add_argument(
+        '--output', '-o', type=str,
+        help='Output file path for calibrated data.')
+
+    parser_calibrate.set_defaults(
+        func=lambda args: calibrate_worms(
+            ARMH7Interface.from_port(args.device),
+            args.file_path,
+            output_path=args.output,
+            update=args.update,
+            inplace=args.inplace))
+
+    parser_read_calib = subparsers.add_parser(
+        'read-calib', help='Read worm gear module calib data from armh7')
+    parser_read_calib.add_argument('--device', '-d', default=None,
+                                   help='Input device port.')
+    parser_read_calib.add_argument(
+        'output_file_path', type=str, help='Output path to yaml file.')
+    parser_read_calib.set_defaults(
+        func=lambda args: read_calib_sensors(
+            ARMH7Interface.from_port(args.device),
+            args.output_file_path))
+
+    parser_print_sensor = subparsers.add_parser(
+        'print-sensor', help='Print sensor values.')
+    parser_print_sensor.add_argument('--device', '-d', default=None,
+                                     help='Input device port.')
+    parser_print_sensor.set_defaults(
+        func=lambda args: print_sensor_values(
+            ARMH7Interface.from_port(args.device)))
+
+    parser_print_worm = subparsers.add_parser(
+        'print-worm', help='Print worm values.')
+    parser_print_worm.add_argument('--device', '-d', default=None,
+                                   help='Input device port.')
+    parser_print_worm.set_defaults(
+        func=lambda args: print_worm_values(
+            ARMH7Interface.from_port(args.device)))
+
+    args = parser.parse_args()
+    if hasattr(args, 'func'):
+        args.func(args)
+    else:
+        parser.print_help()
+
+
+if __name__ == '__main__':
+    main()

rcb4/armh7interface.py

@@ -191,13 +191,11 @@ def from_port(port=None):
 
         interface = ARMH7Interface()
         ret = interface.auto_open()
-        print(ret)
         if ret is True:
             return interface
 
         interface = RCB4Interface()
         ret = interface.auto_open()
-        print(ret)
         if ret is True:
             return interface
 
@@ -761,6 +759,22 @@ def search_worm_ids(self):
         self.worm_sorted_ids = indices
         return indices
 
+    def calibrate_worm(self, worm_idx, servo_idx, sensor_idx,
+                       magenc_present=None):
+        if magenc_present is None:
+            input(f'Servo ID: {servo_idx}, Sensor ID: {sensor_idx}\n'
+                  'Press Enter to confirm the current angle: ')
+            worm = self.memory_cstruct(WormmoduleStruct, worm_idx)
+            magenc_present = worm.magenc_present
+        self.send_worm_calib_data(
+            worm_idx, servo_idx, sensor_idx,
+            module_type=1,
+            magenc_offset=magenc_present)
+        worm = self.memory_cstruct(WormmoduleStruct, worm_idx)
+        print(f'Magnetic encoder offset for Servo ID {servo_idx} '
+              f'set to: {worm.magenc_init}')
+        return worm.magenc_init
+
     def read_worm_angle(self, idx=0):
         if not 0 <= idx < max_sensor_num:
             print(
@@ -883,7 +897,14 @@ def databssram_to_dataflash(self):
         self.cstruct_slot(
             DataAddress, 'data_size',
             self.armh7_address['_ebss'] - self.armh7_address['_sdata'])
-        return self.cfunc_call('dataram_to_dataflash', [])
+
+        # The operation databssram_to_dataflash is time-consuming,
+        # hence the default_timeout is temporarily extended.
+        default_timeout = self._default_timeout
+        self._default_timeout = 5.0
+        ret = self.cfunc_call('dataram_to_dataflash', [])
+        self._default_timeout = default_timeout
+        return ret
 
     def set_sidata(self, sidata=None):
         sidata = sidata or self.armh7_address['_sidata']

requirements.txt

@@ -2,5 +2,7 @@ colorama
 cstruct
 gdown
 numpy
+pandas
 pyelftools
 pyserial
+tabulate

setup.py

@@ -60,6 +60,7 @@
     entry_points={
         "console_scripts": [
             "rcb4-write-firmware=rcb4.apps.write_firmware:main",
+            "armh7-tools=rcb4.apps.armh7_tool:main",
         ],
     },
 )