"""
test_simple_motor.py — Test mínim de la roda dreta per diagnosticar problemes.
Executa des del directori Rasp/tests/:  python3 test_simple_motor.py

Diferències respecte a test_motion.py:
- Usa pi.write() directe en lloc de gpio_trigger()
- Bucle bloquejant amb time.sleep() en lloc de thread
- Sense acceleració, velocitat constant
"""

import pigpio
import time

STEP = 25   # STEP_R_W
DIR  = 23   # DIR_R_W
EN   = 6    # EN_W (actiu LOW)

pi = pigpio.pi()
if not pi.connected:
    print("ERROR: pigpiod no està en marxa")
    exit(1)

pi.set_mode(STEP, pigpio.OUTPUT)
pi.set_mode(DIR,  pigpio.OUTPUT)
pi.set_mode(EN,   pigpio.OUTPUT)

pi.write(EN, 0)    # Activa el driver (LOW = ON)
pi.write(DIR, 1)   # Endavant

# 500 passos/s → període = 1/500 = 2ms → mig període = 1ms
DELAY = 0.001

print("Movent 200 passos endavant...")
for _ in range(200):
    pi.write(STEP, 1)
    time.sleep(DELAY)
    pi.write(STEP, 0)
    time.sleep(DELAY)

print("Fet. Esperant 1s...")
time.sleep(1)

pi.write(DIR, 0)   # Enrere
print("Movent 200 passos enrere...")
for _ in range(200):
    pi.write(STEP, 1)
    time.sleep(DELAY)
    pi.write(STEP, 0)
    time.sleep(DELAY)

print("Fet.")
pi.write(EN, 1)    # Desactiva el driver
pi.stop()