diff --git a/scsservo_sdk_example/ping.py b/scsservo_sdk_example/ping.py index cdc8947..93083d2 100644 --- a/scsservo_sdk_example/ping.py +++ b/scsservo_sdk_example/ping.py @@ -1,21 +1,22 @@ -#!/usr/bin/env python -# -# ********* Ping Example ********* -# -# -# Available SCServo model on this example : All models using Protocol SCS -# This example is tested with a SCServo(STS/SMS/SCS), and an URT -# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000) -# +#!/usr/bin/env python3 import os +import argparse +from scservo_sdk import * # Uses SCServo SDK library + +# ANSI escape codes for colors +COLOR_RESET = "\033[0m" +COLOR_RED = "\033[91m" +COLOR_GREEN = "\033[92m" if os.name == 'nt': import msvcrt def getch(): return msvcrt.getch().decode() else: - import sys, tty, termios + import sys + import tty + import termios fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) def getch(): @@ -26,53 +27,87 @@ def getch(): termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch -from scservo_sdk import * # Uses SCServo SDK library +def ping_servo(device, baudrate, protocol_end, id_start, id_end): + # Initialize PortHandler instance + portHandler = PortHandler(device) + # Initialize PacketHandler instance + packetHandler = PacketHandler(protocol_end) -# Default setting -SCS_ID = 13 # SCServo ID : 1 -BAUDRATE = 1000000 # SCServo default baudrate : 1000000 -DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller - # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + # Open port + if not portHandler.openPort(): + print(f"{COLOR_RED}Failed to open the port: {device}{COLOR_RESET}") + return -protocol_end = 0 # SCServo bit end(STS/SMS=0, SCS=1) + print(f"{COLOR_GREEN}Succeeded to open the port: {device}{COLOR_RESET}") -# Initialize PortHandler instance -# Set the port path -# Get methods and members of PortHandlerLinux or PortHandlerWindows -portHandler = PortHandler(DEVICENAME) + # Set port baudrate + if not portHandler.setBaudRate(baudrate): + print(f"{COLOR_RED}Failed to change the baudrate to {baudrate}{COLOR_RESET}") + portHandler.closePort() + return -# Initialize PacketHandler instance -# Get methods and members of Protocol -packetHandler = PacketHandler(protocol_end) + print(f"{COLOR_GREEN}Succeeded to change the baudrate to {baudrate}{COLOR_RESET}") -# Open port -if portHandler.openPort(): - print("Succeeded to open the port") -else: - print("Failed to open the port") - print("Press any key to terminate...") - getch() - quit() + # Scan for servos + print(f"Scanning for servos in ID range: {id_start}-{id_end}...") + successful_pings = [] # Store successful pings + for scs_id in range(id_start, id_end + 1): # Inclusive of id_end + scs_model_number, scs_comm_result, scs_error = packetHandler.ping(portHandler, scs_id) + if scs_comm_result == COMM_SUCCESS: + print(f"{COLOR_GREEN}[ID:{scs_id:03d}] Ping succeeded. SCServo model number: {scs_model_number}{COLOR_RESET}") + successful_pings.append((scs_id, scs_model_number)) # Store successful ping + elif scs_comm_result != COMM_SUCCESS: + print(f"{COLOR_RED}[ID:{scs_id:03d}] {packetHandler.getTxRxResult(scs_comm_result)}{COLOR_RESET}") + elif scs_error != 0: + print(f"{COLOR_RED}[ID:{scs_id:03d}] {packetHandler.getRxPacketError(scs_error)}{COLOR_RESET}") -# Set port baudrate -if portHandler.setBaudRate(BAUDRATE): - print("Succeeded to change the baudrate") -else: - print("Failed to change the baudrate") - print("Press any key to terminate...") - getch() - quit() + # Close port + portHandler.closePort() -# Try to ping the SCServo -# Get SCServo model number -scs_model_number, scs_comm_result, scs_error = packetHandler.ping(portHandler, SCS_ID) -if scs_comm_result != COMM_SUCCESS: - print("%s" % packetHandler.getTxRxResult(scs_comm_result)) -elif scs_error != 0: - print("%s" % packetHandler.getRxPacketError(scs_error)) -else: - print("[ID:%03d] ping Succeeded. SCServo model number : %d" % (SCS_ID, scs_model_number)) + # Print summary of successful pings + print("\nSummary of Successful Pings:") + if successful_pings: + for scs_id, model_number in successful_pings: + print(f" - {COLOR_GREEN}ID: {scs_id}, Model Number: {model_number}{COLOR_RESET}") + else: + print(f" {COLOR_RED}No successful pings.{COLOR_RESET}") + +if __name__ == "__main__": + # Parse command-line arguments + parser = argparse.ArgumentParser(description="Ping SCServo devices on a given port.") + parser.add_argument( + "--device", + type=str, + default="/dev/tty.usbserial-21240", + help="Device name (e.g., /dev/ttyUSB0 or COM1)", + ) + parser.add_argument( + "--baudrate", + type=int, + default=1000000, + help="Baud rate (default: 1000000)", + ) + parser.add_argument( + "--protocol", + type=int, + default=0, + choices=[0, 1], + help="Protocol end (0 for STS/SMS, 1 for SCS)", + ) + parser.add_argument( + "--id_start", + type=int, + default=1, + help="Start of ID range (default: 1)", + ) + parser.add_argument( + "--id_end", + type=int, + default=253, + help="End of ID range (default: 253)", + ) + args = parser.parse_args() -# Close port -portHandler.closePort() \ No newline at end of file + # Run the ping function with user-provided arguments + ping_servo(args.device, args.baudrate, args.protocol, args.id_start, args.id_end) \ No newline at end of file