Merge pull request #117 from iory/bump

Bump to 0.0.4
iory · Nov 12, 2024 · e190e2d · e190e2d
2 parents b2c5c15 + 3d608c5
commit e190e2d
Show file tree

Hide file tree

Showing 6 changed files with 113 additions and 91 deletions.
diff --git 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
@@ -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()
diff --git a/ros/kxr_controller/package.xml b/ros/kxr_controller/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>kxr_controller</name>
-  <version>0.0.3</version>
+  <version>0.0.4</version>
   <description>The kxr_controller package</description>
 
   <maintainer email="[email protected]">Iori Yanokura</maintainer>

diff --git a/ros/kxr_models/package.xml b/ros/kxr_models/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>kxr_models</name>
-  <version>0.0.3</version>
+  <version>0.0.4</version>
   <description>
     <p>URDF Description package for kxr_models</p>
     <p>This package contains configuration data, 3D models and launch files for kxr robot</p>

diff --git a/ros/kxreus/package.xml b/ros/kxreus/package.xml
@@ -1,7 +1,7 @@
 <package format="3">
 
   <name>kxreus</name>
-  <version>0.0.3</version>
+  <version>0.0.4</version>
   <description>
     roseus interface for kxr robots.
   </description>

diff --git 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":