# =============================================================
# capture_step_N20.py
# Motor CH-N20  |  6V  |  Reducción 1:180  |  PPR_EJE = 1260
# Raspberry Pi Pico — Captura respuesta al escalón
# Salida CSV por USB-Serial → importar en MATLAB
# =============================================================

from machine import Pin, PWM
import time

# ─────────────────────────────────────────────
# PARÁMETROS DE TU MOTOR (ya calculados)
# ─────────────────────────────────────────────
GEAR_RATIO  = 180
PPR_MOTOR   = 7
PPR_EJE     = PPR_MOTOR * GEAR_RATIO   # = 1260

DUTY_ESCALON = 60     # % PWM del escalón — no subas de 70% con 6V
DURACION_MS  = 10000   # 8 segundos de captura
TS_MS        = 100     # 50ms de muestreo — necesario con PPR alto

# ─────────────────────────────────────────────
# PINES
# ─────────────────────────────────────────────
PIN_HALL_A = 2
PIN_HALL_B = 3
PIN_PWM    = 15
PIN_DIR_FW = 14
PIN_DIR_BW = 13

# ─────────────────────────────────────────────
# HARDWARE
# ─────────────────────────────────────────────
hall_a = Pin(PIN_HALL_A, Pin.IN, Pin.PULL_UP)
hall_b = Pin(PIN_HALL_B, Pin.IN, Pin.PULL_UP)

pwm    = PWM(Pin(PIN_PWM));  pwm.freq(10_000)
dir_fw = Pin(PIN_DIR_FW, Pin.OUT)
dir_bw = Pin(PIN_DIR_BW, Pin.OUT)

# ─────────────────────────────────────────────
# ENCODER
# ─────────────────────────────────────────────
encoder_count = 0

def hall_isr(pin):
    global encoder_count
    if hall_b.value():
        encoder_count += 1
    else:
        encoder_count -= 1

hall_a.irq(trigger=Pin.IRQ_RISING, handler=hall_isr)

# ─────────────────────────────────────────────
# MOTOR
# ─────────────────────────────────────────────
def motor_adelante(duty_pct):
    dir_fw.value(1); dir_bw.value(0)
    pwm.duty_u16(int(duty_pct / 100.0 * 65535))

def motor_stop():
    dir_fw.value(0); dir_bw.value(0)
    pwm.duty_u16(0)

# ─────────────────────────────────────────────
# CAPTURA
# ─────────────────────────────────────────────
print("# CH-N20 6V 1:180 | PPR_EJE=1260 | Duty={}% | Ts={}ms".format(
      DUTY_ESCALON, TS_MS))
print("# Esperando 1s para estabilizar...")
time.sleep(1)

print("tiempo_ms,encoder_pulsos,velocidad_rpm,posicion_grados")


motor_adelante(DUTY_ESCALON)
t0         = time.ticks_ms()
prev_count = 0
prev_t_ms  = 0

while True:
    time.sleep_ms(TS_MS)

    ahora_ms     = time.ticks_ms()
    t_ms         = time.ticks_diff(ahora_ms, t0)
    dt_s         = (t_ms - prev_t_ms) / 1000.0
    count_actual = encoder_count
    delta_p      = count_actual - prev_count

    rpm    = (delta_p / PPR_EJE) * (60.0 / dt_s) if dt_s > 0 else 0.0
    grados = (count_actual / PPR_EJE) * 360.0

    print("{},{},{:.3f},{:.3f}".format(t_ms, count_actual, rpm, grados))

    prev_count = count_actual
    prev_t_ms  = t_ms

    if t_ms >= DURACION_MS:
        break

motor_stop()
print("# FIN")