# EV-1 transportband met stappenmotor
import time
from machine import Pin

# outputs
led_14 = Pin(14, Pin.OUT)
led_27 = Pin(27, Pin.OUT)
led_26 = Pin(26, Pin.OUT)
led_25 = Pin(25, Pin.OUT)

#inputs
knop_begin = Pin(15, Pin.IN, Pin.PULL_UP)
knop_start = Pin(16, Pin.IN, Pin.PULL_UP)
knop_pos1 = Pin(17, Pin.IN, Pin.PULL_UP)
knop_pos2 = Pin(18, Pin.IN, Pin.PULL_UP)

# hulpveranderlijken voor flankdetectie
knop_begin_vorige = 1
knop_start_vorige = 1
knop_pos1_vorige = 1
knop_pos2_vorige = 1

# maak lists (array) van input en output objecten
leds = [led_14, led_27, led_26, led_25]
knoppen = [knop_begin, knop_start, knop_pos1, knop_pos2]

# vaste posities op transportband
pos0 = 0
pos1 = 500
pos2 = 1000

huidige_pos = 0
target = 0

print("start test transportband v5")

last_step = 0
def stuur_motor( aantal_stappen , cw_acw ):
    global last_step, huidige_pos
    # maak last step laag en zet volgende stap in de juiste richting
    for _ in range(aantal_stappen):
        if cw_acw == "cw":
            huidige_pos +=1
            next_step = last_step + 1
            if next_step >= 4:
                next_step = 0

        else:
            huidige_pos -=1            
            next_step = last_step - 1
            if next_step < 0:
                next_step = 3

        # stuur last 0 next 1
        leds[last_step].value(0)
        leds[next_step].value(1)  
        last_step = next_step
        time.sleep(0.0025)

        print("huidige_pos =", huidige_pos)


# zorg ervoor dat de slede naar het begin wordt gestuurd voor calibratie
while True:
    if knop_begin.value() == 0:
        print("transportband staat aan het begin, calibratie ok")
        huidige_pos = 0
        target = 0
        break
    else:
        stuur_motor(1, "acw")

# transportband staat nu in de beginpositie, calibratie OK
while True:
    toestand = knop_start.value()
    if toestand != knop_start_vorige:
        knop_start_vorige = toestand
        if toestand == 0:
            print("knop ga naar start ingedrukt")
            target = 0
        time.sleep(0.005) # dender laten passeren

    toestand = knop_pos1.value()
    if toestand != knop_pos1_vorige:
        knop_pos1_vorige = toestand
        if toestand == 0:
            print("knop ga naar pos1 ingedrukt")
            target = 500
        time.sleep(0.005) # dender laten passeren

    toestand = knop_pos2.value()
    if toestand != knop_pos2_vorige:
        knop_pos2_vorige = toestand
        if toestand == 0:
            print("knop ga naar pos2 ingedrukt")
            target = 1000
        time.sleep(0.005) # dender laten passeren

    if huidige_pos > target:
        stuur_motor(1,"acw")
    elif huidige_pos < target :
        stuur_motor(1,"cw")