diff --git a/mini_bdx_runtime/mini_bdx_runtime/utils.py b/mini_bdx_runtime/mini_bdx_runtime/utils.py new file mode 100644 index 0000000..e958350 --- /dev/null +++ b/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 ' 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 diff --git a/scripts/configure_motor.py b/scripts/configure_motor.py index 2110d78..d62a73c 100644 --- a/scripts/configure_motor.py +++ b/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 ' 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) diff --git a/setup.cfg b/setup.cfg index 0eb07fa..3f00a52 100644 --- a/setup.cfg +++ b/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