improving and cleaning stuff

mini_bdx_runtime/mini_bdx_runtime/utils.py

@@ -0,0 +1,51 @@
+import serial.tools.list_ports
+from pypot.feetech import FeetechSTS3215IO
+
+DEVICE_IDS = [
+    (0x1A86, 0x55D3),  # Example device 1
+    # Add more (vendor_id, product_id) pairs here as needed
+]
+
+FALLBACK_PORT = "/dev/ttyACM0"
+
+
+def get_port():
+    port = None
+    auto_port = auto_detect_port()
+    if auto_port is None:
+        print(
+            f"Device not auto-detected. Attempting connection using fallback port {FALLBACK_PORT}."
+        )
+        port = FALLBACK_PORT
+    else:
+        port = auto_port
+
+    # Attempt to connect and catch any connection errors.
+    try:
+        FeetechSTS3215IO(port)
+        return port
+    except Exception as exc:
+        message = f"Error connecting to the motor using port {port}: {exc}. Please check your connection.\n"
+        # If the fallback port was used, advise the user with additional usage information.
+        if port == FALLBACK_PORT:
+            message += f" If your device is not connected via {FALLBACK_PORT}, please specify the correct port using the '--port' argument (e.g., --port /dev/ttyUSB0)."
+        else:
+            message += " If you believe your device is connected on a different port, try specifying it using the '--port <PORT>' argument."
+        print(message)
+    return None
+
+
+def auto_detect_port():
+    """
+    Scans available serial ports and returns the port name for the first device
+    matching any of the given (vendor_id, product_id) pairs.
+    """
+    ports = list(serial.tools.list_ports.comports())
+    for port in ports:
+        for vendor_id, product_id in DEVICE_IDS:
+            if port.vid == vendor_id and port.pid == product_id:
+                print(
+                    f"Found device on port: {port.device} (VID: {hex(vendor_id)}, PID: {hex(product_id)})"
+                )
+                return port.device
+    return None

scripts/configure_motor.py

@@ -1,78 +1,41 @@
 from pypot.feetech import FeetechSTS3215IO
 import argparse
 import time
-import serial.tools.list_ports
+from mini_bdx_runtime.utils import get_port
+import tqdm
 
 DEFAULT_ID = 1  # A brand new motor should have id 1
 
-# These are placeholders. Please replace these with your actual device's USB IDs.
-TARGET_VENDOR_ID = 0x1A86  # e.g., your device's vendor id (in hex)
-TARGET_PRODUCT_ID = 0x55D3  # e.g., your device's product id (in hex)
-
-# Update the find_port function to accept a list of (vendor_id, product_id) tuples.
-def find_port(device_ids):
-    """
-    Scans available serial ports and returns the port name for the first device
-    matching any of the given (vendor_id, product_id) pairs.
-    """
-    ports = list(serial.tools.list_ports.comports())
-    for port in ports:
-        for vendor_id, product_id in device_ids:
-            if port.vid == vendor_id and port.pid == product_id:
-                print(f"Found device on port: {port.device} (VID: {hex(vendor_id)}, PID: {hex(product_id)})")
-                return port.device
-    return None
-
-# Update the script to use a list of device IDs.
-DEVICE_IDS = [
-    (0x1A86, 0x55D3),  # Example device 1
-    # Add more (vendor_id, product_id) pairs here as needed
-]
-
 # Parse arguments. This allows an override if you supply --port.
 parser = argparse.ArgumentParser()
 parser.add_argument(
     "--port",
-    help=("The port the motor is connected to. If not specified, "
-          "the script will try to auto-detect the port using provided USB IDs or "
-          "fall back to /dev/ttyACM0."),
+    help=(
+        "The port the motor is connected to. If not specified, "
+        "the script will try to auto-detect the port using provided USB IDs or "
+        "fall back to /dev/ttyACM0."
+    ),
     default=None,
 )
 parser.add_argument("--id", help="The id to set to the motor.", type=str, required=True)
 args = parser.parse_args()
 
+port = args.port
 # If no port is provided, try to auto-detect using the USB id info.
-if args.port is None:
-    auto_port = find_port(DEVICE_IDS)
-    if (auto_port is None):
-        fallback_port = "/dev/ttyACM0"
-        print(f"Device not auto-detected. Attempting connection using fallback port {fallback_port}.")
-        args.port = fallback_port
-    else:
-        args.port = auto_port
+if port is None:
+    port = get_port()
+if port is None:
+    exit()
 
-# Attempt to connect and catch any connection errors.
-try:
-    io = FeetechSTS3215IO(args.port)
-except Exception as exc:
-    message = f"Error connecting to the motor using port {args.port}: {exc}. Please check your connection.\n"
-    # If the fallback port was used, advise the user with additional usage information.
-    fallback_port = "/dev/ttyACM0"
-    if args.port == fallback_port:
-        message += f" If your device is not connected via {fallback_port}, please specify the correct port using the '--port' argument (e.g., --port /dev/ttyUSB0)."
-    else:
-        message += " If you believe your device is connected on a different port, try specifying it using the '--port <PORT>' argument."
-    print(message)
-    exit(1)
+
+io = FeetechSTS3215IO(port)
 
 current_id = DEFAULT_ID
 
 
 def scan():
     id = None
-    for i in range(255):
-
-        print(f"scanning for id {i} ...")
+    for i in tqdm.tqdm(range(255), desc="Scanning ...", unit="id"):
         try:
             io.get_present_position([i])
             id = i
@@ -90,6 +53,7 @@ except Exception:
         f"Could not find motor with default id ({DEFAULT_ID}). Scanning for motor ..."
     )
     res = scan()
+    print("ID :", res)
     if res is not None:
         current_id = res
     else:
@@ -97,8 +61,6 @@ except Exception:
         exit()
 
 
-# print("current id: ", current_id)
-
 kp = io.get_P_coefficient([current_id])
 ki = io.get_I_coefficient([current_id])
 kd = io.get_D_coefficient([current_id])
@@ -106,11 +68,6 @@ max_acceleration = io.get_maximum_acceleration([current_id])
 acceleration = io.get_acceleration([current_id])
 mode = io.get_mode([current_id])
 
-# print(f"PID : {kp}, {ki}, {kd}")
-# print(f"max_acceleration: {max_acceleration}")
-# print(f"acceleration: {acceleration}")
-# print(f"mode: {mode}")
-
 io.set_lock({current_id: 0})
 io.set_mode({current_id: 0})
 io.set_maximum_acceleration({current_id: 0})
@@ -123,7 +80,13 @@ io.change_id({current_id: int(args.id)})
 current_id = int(args.id)
 
 time.sleep(1)
+print("==")
+res = input("WARNING, the motor will move to its zero position. Continue ? ([Y]/n)")
+if res.lower() != "y" and res.lower() != "":
+    print("Exiting ...")
+    exit()
 
+print("==")
 io.set_goal_position({current_id: 0})
 
 time.sleep(1)

setup.cfg

@@ -24,6 +24,7 @@ install_requires =
     pygame==2.6.0
     pypot @ git+https://github.com/pollen-robotics/pypot@support-feetech-sts3215
     openai==1.70.0
+    tqdm
 
     # adafruit_extended_bus