From 63ef0fdf46894f35b0bac011a7e50e3b9aad9cc3 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 15:38:28 +0900 Subject: [PATCH 1/2] Modified --- README.md | 8 ++ rcb4/apps/ics_manager.py | 2 +- rcb4/ics.py | 194 ++++++++++++++++++++++----------------- 3 files changed, 117 insertions(+), 87 deletions(-) diff --git a/README.md b/README.md index 01343f1c..88999daf 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,14 @@ Connect st-link and run the following command. rcb4-write-firmware ``` +## Change Servo ID + +You can use ics-manager command to change servo id. + +```bash +ics-manager +``` + ## Contributing ### Automatic Formatting diff --git a/rcb4/apps/ics_manager.py b/rcb4/apps/ics_manager.py index 7c9b38e8..b1c6709c 100644 --- a/rcb4/apps/ics_manager.py +++ b/rcb4/apps/ics_manager.py @@ -26,7 +26,7 @@ def parse_args(): """Parse command-line arguments.""" parser = argparse.ArgumentParser(description="ICS Servo Controller CLI Tool") parser.add_argument( - "--yaml_path", + "--yaml-path", default=None, help="Path to YAML configuration file for servo settings" ) diff --git a/rcb4/ics.py b/rcb4/ics.py index d5920a24..ff008de7 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -91,6 +91,9 @@ def __init__(self, baudrate=1250000, yaml_path=None): def open_connection(self): ports = serial.tools.list_ports.comports() + if len(ports) == 0: + print(f"{Fore.RED}No USB Found.{Style.RESET_ALL}") + print(f"{Fore.RED}May Dual USB Adapter is wrong.{Style.RESET_ALL}") for p in ports: if p.vid == 0x165C and p.pid == 0x08: for baudrate in [115200, 625000, 1250000]: @@ -141,7 +144,8 @@ def get_servo_id(self): self.ics.write(bytes([0xFF, 0x00, 0x00, 0x00])) time.sleep(0.1) ret = self.ics.read(5) - return ret[4] & 0x1F + servo_id = ret[4] & 0x1F + return servo_id def set_servo_id(self, servo_id): self.ics.write(bytes([0xE0 | (0x1F & servo_id), 0x01, 0x01, 0x01])) @@ -276,6 +280,7 @@ def set_param(self, ics_param64, servo_id=None): def close_connection(self): if self.ics and self.ics.is_open: self.ics.close() + self.ics = None def display_status(self): options = [ @@ -291,6 +296,7 @@ def display_status(self): selectable_options = ["Current Servo ID", "Angle"] try: use_previous_result = False + previous_servo_id = None while True: if not self.ics or not self.ics.is_open: # Clear the previous output at the start of each loop @@ -323,94 +329,105 @@ def display_status(self): sys.stdout.flush() # Print servo status - print("--- Servo Status ---") - if use_previous_result is False: - _, result = self.read_param() - for i, option in enumerate(options): - if i == self.selected_index: - print( - f">> {option}: {self.get_status(option, result, selected=True)}" - ) - else: - print(f" {option}: {self.get_status(option, result, selected=False)}") - print("----------------------\n") - print( - "Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles" - ) - print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.") - print("Press 'z' to reset servo position") - print( - "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)" - ) - print("Press 'f' to set free mode\n") - print("'q' to quit.") - - use_previous_result = False - - # Get key input without Enter - key = readchar.readkey() - - # Perform actions based on key - if key == "z": - self.reset_servo_position() - elif key == "r": - self.toggle_rotation_mode() - elif key == "f": - self.set_free_mode() - elif key == "q": - print("Exiting...") - break - elif key == readchar.key.UP: - self.selected_index = (self.selected_index - 1) % len( - selectable_options + try: + servo_id = self.get_servo_id() + if servo_id != previous_servo_id: + use_previous_result = False + previous_servo_id = servo_id + print("--- Servo Status ---") + if use_previous_result is False: + _, result = self.read_param() + for i, option in enumerate(options): + if i == self.selected_index: + print( + f">> {option}: {self.get_status(option, result, selected=True)}" + ) + else: + print(f" {option}: {self.get_status(option, result, selected=False)}") + + print("----------------------\n") + print( + "Use ↑↓ to navigate, ←→ to adjust Current Servo ID or Servo Angles" ) - use_previous_result = True - elif key == readchar.key.DOWN: - self.selected_index = (self.selected_index + 1) % len( - selectable_options + print(f"Press 'Enter' when Current Servo ID is selected to save the currently highlighted ID in {Fore.GREEN}green{Style.RESET_ALL}.") + print("Press 'z' to reset servo position") + print( + "Press 'r' to toggle rotation mode (enables continuous wheel-like rotation)" ) - use_previous_result = True - elif ( - key == readchar.key.ENTER - and selectable_options[self.selected_index] == "Current Servo ID" - ): - self.set_servo_id(self.servo_id) - time.sleep(0.3) - if self.servo_id_to_rotation is not None: - self.set_rotation(self.servo_id_to_rotation[self.servo_id]) - time.sleep(0.3) - elif ( - key == readchar.key.LEFT - and selectable_options[self.selected_index] == "Current Servo ID" - ): - use_previous_result = True - if self.servo_id_index == 0: - self.servo_id_index = len(self.servo_candidates) - 1 - else: - self.servo_id_index = max(0, self.servo_id_index - 1) - self.servo_id = self.servo_candidates[self.servo_id_index] - elif ( - key == readchar.key.RIGHT - and selectable_options[self.selected_index] == "Current Servo ID" - ): - use_previous_result = True - if self.servo_id_index == len(self.servo_candidates) - 1: - self.servo_id_index = 0 - else: - self.servo_id_index = min( - len(self.servo_candidates) - 1, self.servo_id_index + 1 + print("Press 'f' to set free mode\n") + print("'q' to quit.") + + use_previous_result = False + + # Get key input without Enter + key = readchar.readkey() + + # Perform actions based on key + if key == "z": + self.reset_servo_position() + elif key == "r": + self.toggle_rotation_mode() + elif key == "f": + self.set_free_mode() + elif key == "q": + print("Exiting...") + break + elif key == readchar.key.UP: + self.selected_index = (self.selected_index - 1) % len( + selectable_options + ) + use_previous_result = True + elif key == readchar.key.DOWN: + self.selected_index = (self.selected_index + 1) % len( + selectable_options ) - self.servo_id = self.servo_candidates[self.servo_id_index] - elif ( - key == readchar.key.LEFT - and selectable_options[self.selected_index] == "Angle" - ): - self.decrease_angle() - elif ( - key == readchar.key.RIGHT - and selectable_options[self.selected_index] == "Angle" - ): - self.increase_angle() + use_previous_result = True + elif ( + key == readchar.key.ENTER + and selectable_options[self.selected_index] == "Current Servo ID" + ): + self.set_servo_id(self.servo_id) + time.sleep(0.3) + if self.servo_id_to_rotation is not None: + self.set_rotation(self.servo_id_to_rotation[self.servo_id]) + time.sleep(0.3) + elif ( + key == readchar.key.LEFT + and selectable_options[self.selected_index] == "Current Servo ID" + ): + use_previous_result = True + if self.servo_id_index == 0: + self.servo_id_index = len(self.servo_candidates) - 1 + else: + self.servo_id_index = max(0, self.servo_id_index - 1) + self.servo_id = self.servo_candidates[self.servo_id_index] + elif ( + key == readchar.key.RIGHT + and selectable_options[self.selected_index] == "Current Servo ID" + ): + use_previous_result = True + if self.servo_id_index == len(self.servo_candidates) - 1: + self.servo_id_index = 0 + else: + self.servo_id_index = min( + len(self.servo_candidates) - 1, self.servo_id_index + 1 + ) + self.servo_id = self.servo_candidates[self.servo_id_index] + elif ( + key == readchar.key.LEFT + and selectable_options[self.selected_index] == "Angle" + ): + self.decrease_angle() + elif ( + key == readchar.key.RIGHT + and selectable_options[self.selected_index] == "Angle" + ): + self.increase_angle() + except Exception as e: + print(f'[ERROR] {e}') + use_previous_result = False + self.close_connection() + continue # Flush the output to ensure it displays correctly sys.stdout.flush() @@ -511,3 +528,8 @@ def _4bit2num(self, lst, v): for i in lst: sum_val = (sum_val << 4) + (v[i - 1] & 0x0F) return sum_val + + +if __name__ == '__main__': + servo_controller = ICSServoController() + servo_controller.open_connection() From 3d608c50449ac812c0f292a7351d394fd979c825 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 16:17:44 +0900 Subject: [PATCH 2/2] 0.0.4 --- ros/kxr_controller/package.xml | 2 +- ros/kxr_models/package.xml | 2 +- ros/kxreus/package.xml | 2 +- setup.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ros/kxr_controller/package.xml b/ros/kxr_controller/package.xml index 9219cb84..12379313 100644 --- a/ros/kxr_controller/package.xml +++ b/ros/kxr_controller/package.xml @@ -1,7 +1,7 @@ kxr_controller - 0.0.3 + 0.0.4 The kxr_controller package Iori Yanokura diff --git a/ros/kxr_models/package.xml b/ros/kxr_models/package.xml index fb958d2a..7ae80c24 100644 --- a/ros/kxr_models/package.xml +++ b/ros/kxr_models/package.xml @@ -1,6 +1,6 @@ kxr_models - 0.0.3 + 0.0.4

URDF Description package for kxr_models

This package contains configuration data, 3D models and launch files for kxr robot

diff --git a/ros/kxreus/package.xml b/ros/kxreus/package.xml index 22411cad..4a8c02ef 100644 --- a/ros/kxreus/package.xml +++ b/ros/kxreus/package.xml @@ -1,7 +1,7 @@ kxreus - 0.0.3 + 0.0.4 roseus interface for kxr robots. diff --git a/setup.py b/setup.py index 9b1309db..e1edcc33 100644 --- a/setup.py +++ b/setup.py @@ -6,7 +6,7 @@ from setuptools import find_packages from setuptools import setup -version = "0.0.3" +version = "0.0.4" if sys.argv[-1] == "release":