from machine import Pin
import rp2
import time

power_pin = Pin(17, Pin.IN, Pin.PULL_UP)
dir_pin = Pin(16, Pin.IN, Pin.PULL_UP)

@rp2.asm_pio(set_init=(rp2.PIO.OUT_LOW,)*4)
def stepper_cw():
    # 順時針驅動序列
    set(pins, 0b1001)
    set(x, 31)[6]
    label("a")
    nop()[29]
    jmp(x_dec, "a")
    set(pins, 0b0011)
    set(x, 31)[6]
    label("b")
    nop()[29]
    jmp(x_dec, "b")
    set(pins, 0b0110)
    set(x, 31)[6]
    label("c")
    nop()[29]
    jmp(x_dec, "c")
    set(pins, 0b1100)
    set(x, 31)[6]
    label("d")
    nop()[29]
    jmp(x_dec, "d")

@rp2.asm_pio(set_init=(rp2.PIO.OUT_LOW,)*4)
def stepper_ccw():
    # 逆時針驅動序列，順序反轉
    set(pins, 0b1001)
    set(x, 31)[6]
    label("a")
    nop()[29]
    jmp(x_dec, "a")
    set(pins, 0b0110)
    set(x, 31)[6]
    label("b")
    nop()[29]
    jmp(x_dec, "b")
    set(pins, 0b0011)
    set(x, 31)[6]
    label("c")
    nop()[29]
    jmp(x_dec, "c")
    set(pins, 0b1100)
    set(x, 31)[6]
    label("d")
    nop()[29]
    jmp(x_dec, "d")

sm_cw = rp2.StateMachine(0, stepper_cw, freq=4000, set_base=Pin(12))
sm_ccw = rp2.StateMachine(1, stepper_ccw, freq=4000, set_base=Pin(12))

sm_cw.active(0)
sm_ccw.active(0)

while True:
    if power_pin.value()==0:
        if dir_pin.value() == 0:
            sm_ccw.active(1)
            sm_cw.active(0)
        else:
            sm_cw.active(1)
            sm_ccw.active(0)
    else:
        sm_cw.active(0)
        sm_ccw.active(0)        
    time.sleep(0.1)