import serial
import time

try:
    puerto = serial.Serial('/dev/ttyAMA0', 921600, timeout=1.0)
    print("Enviando comando de calibracion C:")
    puerto.write(b"C:\n")
    time.sleep(1)
    
    # Leer todo lo que devuelva
    lines = []
    end_time = time.time() + 2
    while time.time() < end_time:
        if puerto.in_waiting:
            lines.append(puerto.readline().decode('utf-8', errors='ignore').strip())
            
    for l in lines[-10:]: # Mostrar ultimas 10 lineas para no saturar
        print("RX:", l)

    print("Enviando comando A:1.000,1.000,1.000,1.000 (100% velocidad)")
    puerto.write(b"A:1.000,1.000,1.000,1.000\n")
    time.sleep(0.5)
    
    # Leer telemetria para ver si incrementan los ticks
    ticks_lines = []
    end_time = time.time() + 1
    while time.time() < end_time:
        if puerto.in_waiting:
            line = puerto.readline().decode('utf-8', errors='ignore').strip()
            if line.startswith("T:"):
                ticks_lines.append(line)
                
    for l in ticks_lines[-5:]:
        print("TICKS:", l)
        
    puerto.write(b"S:\n")
    print("Stop enviado.")
    
except Exception as e:
    print("Error:", e)