Added scanning functionality and coloring

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()
+    # Run the ping function with user-provided arguments
+    ping_servo(args.device, args.baudrate, args.protocol, args.id_start, args.id_end)