thisfile="left90Old.py"
from machine import *
import time
from  UKMARS import *

lwheel = Pin(13,Pin.IN,Pin.PULL_UP)
rwheel = Pin(14,Pin.IN,Pin.PULL_UP)
led = Pin(25,Pin.OUT)

LDistance=0
RDistance=0
    
def Lwheelirq(which):
    global LDistance          # count of wheel markers
    ledL.value(lwheel.value())
#led.toggle()
    LDistance += 1

def Rwheelirq(which):
    global RDistance          # count of wheel markers
    ledR.value(rwheel.value())
    led.toggle()
    RDistance += 1

              # set up interrupt handler for pin stp as stpirq()
lwheel.irq(handler=Lwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING) 
rwheel.irq(handler=Rwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING) 

'''
while True:
    ledR.value(rwheel.value())
    ledL.value(lwheel.value())
    print (LDistance,RDistance)
    time.sleep(.1)
'''

LM=Motor("L")
RM=Motor("R")

while True:
    while (Button.value()==0):
        time.sleep(.1)
    time.sleep(2)
    
    RM.speed(20)
    LM.speed(0)
    ang=int(readswitches()/10)
    print(ang)
    
    Angle = RDistance+19

    while (RDistance <Angle):
        time.sleep(0.01)

    RM.speed(0)
    LM.speed(0)