test_servos.py

#!/usr/bin/env python3
"""
Servo test utility for hexapod crawler chassis
Sends commands to test servo functionality
"""

import serial
import time
import sys

def main():
    port = sys.argv[1] if len(sys.argv) > 1 else '/dev/ttyACM0'
    
    print(f"Connecting to {port}...")
    
    try:
        ser = serial.Serial(port, 115200, timeout=1)
        time.sleep(0.5)  # Wait for connection
        
        print("Connected! Reading startup messages...")
        
        # Read initial messages
        for _ in range(10):
            if ser.in_waiting:
                line = ser.readline().decode('utf-8', errors='ignore').strip()
                if line:
                    print(line)
        
        # Auto-wiggle RR_TIBIA (servo 2) on startup for identification
        print("\n>>> Wiggling RR_TIBIA (servo 2 - Rear Right toe) for identification...")
        ser.write(b'1')
        time.sleep(0.1)
        
        # Read responses
        timeout = time.time() + 5
        while time.time() < timeout:
            if ser.in_waiting:
                line = ser.readline().decode('utf-8', errors='ignore').strip()
                if line:
                    print(line)
                if "wiggle" in line.lower() or "complete" in line.lower():
                    break
            time.sleep(0.01)
        
        print("\n=== Interactive Servo Test ===")
        print("Commands:")
        print("  1 - Wiggle RR_TIBIA (servo 2) - Rear Right toe")
        print("  c - Center all servos (1500µs)")
        print("  s - Sweep all servos (test each one)")
        print("  h - Show help on device")
        print("  q - Quit")
        print()
        
        while True:
            cmd = input("Enter command: ").strip().lower()
            
            if cmd == 'q':
                print("Exiting...")
                break
            elif cmd in ['1', 'c', 's', 'h']:
                ser.write(cmd.encode())
                time.sleep(0.1)
                
                # Read responses
                timeout = time.time() + 2
                while time.time() < timeout:
                    if ser.in_waiting:
                        line = ser.readline().decode('utf-8', errors='ignore').strip()
                        if line:
                            print(line)
                    time.sleep(0.01)
                
                # For sweep or wiggle, read continuously
                if cmd in ['s', '1']:
                    action = "Sweep test" if cmd == 's' else "Wiggle test"
                    print(f"\n{action} running...")
                    timeout = time.time() + 60  # 60 second timeout
                    while time.time() < timeout:
                        if ser.in_waiting:
                            line = ser.readline().decode('utf-8', errors='ignore').strip()
                            if line:
                                print(line)
                                if "complete" in line.lower():
                                    break
                        time.sleep(0.01)
            else:
                print("Invalid command. Use 1, c, s, h, or q")
        
        ser.close()
        
    except serial.SerialException as e:
        print(f"Error: {e}")
        print("\nMake sure:")
        print("  1. Board is connected and flashed with servo test firmware")
        print("  2. You have permission: sudo usermod -a -G dialout $USER")
        print(f"  3. Port {port} is correct")
        return 1
    except KeyboardInterrupt:
        print("\nInterrupted by user")
        return 0

if __name__ == '__main__':
    sys.exit(main())