from drv8825 import DRV8825
from machine import Pin
from time import sleep_ms

PIN_STEP = 26
PIN_DIR  = 27
PIN_SW   = 28

stepper = DRV8825(PIN_STEP, direction_pin=PIN_DIR)
home_sw = Pin(PIN_SW, Pin.IN, Pin.PULL_UP)

stepper_position = None

def stop_on_sw_0(stepper):
    if home_sw.value() == 0:
        stepper.stop()

def stop_on_sw_1(stepper):
    if home_sw.value() == 1:
        stepper.stop()

def wait_until_stepper_done(stepper):
    while stepper.is_running():
        sleep_ms(100)

def home():
    global stepper_position

    stepper = DRV8825(PIN_STEP, direction_pin=PIN_DIR, timer_callback=stop_on_sw_0)
    stepper.freerun(stepfreq=-1000)
    wait_until_stepper_done(stepper)

    stepper = DRV8825(PIN_STEP, direction_pin=PIN_DIR, timer_callback=stop_on_sw_1)
    stepper.freerun(stepfreq=1000)
    wait_until_stepper_done(stepper)

    stepper_position = 0

TRACK_LENGTH_STEPS = 96880
TRACK_LENGTH_MM = 600
STEPS_PER_MM = int(TRACK_LENGTH_STEPS / TRACK_LENGTH_MM)

def goto(position_in_mm):
    global stepper_position
    new_position = STEPS_PER_MM * position_in_mm

    if stepper_position is None:
        home()

    steps = new_position - stepper_position
    stepper = DRV8825(PIN_STEP, direction_pin=PIN_DIR)
    stepper.steps(steps, stepfreq=1000)
    wait_until_stepper_done(stepper)

    stepper_position = new_position