From ef79ccf1ad59c816ac4fffe32ed03236a9af39b4 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Tue, 12 Nov 2024 15:38:28 +0900 Subject: [PATCH] Modified --- README.md | 8 ++ rcb4/apps/ics_manager.py | 2 +- rcb4/ics.py | 184 +++++++++++++++++++++------------------ 3 files changed, 107 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..02f67685 100644 --- a/rcb4/ics.py +++ b/rcb4/ics.py @@ -141,7 +141,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 +277,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 = [ @@ -323,94 +325,99 @@ 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: + 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: + self.close_connection() + continue # Flush the output to ensure it displays correctly sys.stdout.flush() @@ -511,3 +518,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()