from machine import Pin
from time import sleep_ms

led_rouge = Pin(19, Pin.OUT)
led_vert = Pin(20, Pin.OUT)
led_jaune = Pin(18, Pin.OUT)

direction = Pin(0, Pin.OUT)
step = Pin(1, Pin.OUT)
ms1 = Pin(4, Pin.OUT)
ms2 = Pin(3, Pin.OUT)
ms3 = Pin(2, Pin.OUT)
enable = Pin(5, Pin.OUT)       

button_rouge = Pin(13, Pin.IN, Pin.PULL_UP)     
button_vert = Pin(12, Pin.IN, Pin.PULL_UP)      
button_jaune = Pin(11, Pin.IN, Pin.PULL_UP)     
button_stop = Pin(9, Pin.IN, Pin.PULL_UP)       
button_blanc = Pin(10, Pin.IN, Pin.PULL_UP)     

compteur = 0                                    


capteur_gauche = Pin(8, Pin.IN, Pin.PULL_UP)     
capteur_droite = Pin(7, Pin.IN, Pin.PULL_UP)     

def tourner_sens_horaire():
    direction.value(1)
    step.value(1)
    led_vert.value(1)
    sleep_ms(delay)
    step.value(0)
    sleep_ms(delay)
        
def tourner_sens_trigonometrique():
    direction.value(0)
    step.value(1)
    led_rouge.value(1)
    sleep_ms(delay)
    step.value(0)
    sleep_ms(delay)

def LED_eteintes():
    led_vert.value(0)
    led_rouge.value(0)

def eteindre_driver():
    enable.value(1)       

def allumer_driver():
    enable.value(0)       

    
led_rouge.value(0)
led_vert.value(0)
led_jaune.value(0)
    
direction.value(0)
step.value(0)
ms1.value(0)
ms2.value(0)
ms3.value(0)
enable.value(0)       
    
delay = 1       
pas = 0



while True:  
    if button_jaune.value() == 0:
        print("EMERGENCY STOP!! You need to reset the machine")
        eteindre_driver()
        led_jaune.value(1)
        compteur = 1
    if button_blanc.value() == 0:
        print("Machine reset")
        allumer_driver()
        led_jaune.value(0)
        compteur = 0
    if button_rouge.value() == 0 and button_vert.value() == 1 and compteur == 0:
        while not capteur_gauche.value() == 0 and not button_jaune.value() == 0 and not button_stop.value() ==0:
            tourner_sens_trigonometrique()
            pas -= 1
            degre = pas * 360/200
            print("tourner_sens_trigonométrique():", pas, "not, which corresponds to", degre, "°" )
    if button_rouge.value() == 1 and button_vert.value() == 0 and compteur == 0:
        while not capteur_droite.value() == 0 and not button_jaune.value() == 0 and not button_stop.value() ==0:
            tourner_sens_horaire()
            pas += 1
            degre = pas * 360/200
            print("tourner_sens_horaire():", pas, "not, which corresponds to", degre, "°" )
    else:
        LED_eteintes()
    
    if button_stop.value() == 0:
        print("STOP")

    if capteur_gauche.value() == 0:
        print("STOP!! Left sensor engaged")
    if capteur_droite.value() == 0:
        print("STOP!! Left sensor engaged")
    
    if button_rouge.value() == 0 and compteur == 1:
        print("You need to reset (white button)")
    if button_vert.value() == 0 and compteur == 1:
        print("You need to reset (white button)")