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)

RDistance=0
    
def rwheelirq(which):
    global RDistance          # count of wheel markers
    #if (wheel.value() == 0):     # Black
    #    led.off()             # side LED
    #else:                      # white
    #    led.on()              # side LED
    #    Distance += 1
    #print (which)
    led.toggle()
    RDistance += 1



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


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

print ("Speed 20")
RM.speed(40)
LM.speed(40)


while (RDistance <50):
    led.value(rwheel.value())
    print(RDistance)
    time.sleep(0.11)
   

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


while True:
    print(RDistance)
    time.sleep(1)
    
    
    
'''
    print(fred.value())
    if (fred.value()==1):
        led.on()
    else:
        led.off()

PWMA = PWM(Pin(11))
PWMA.freq(1000)
DIRA = PWM(Pin(9))
DIRA.freq(1000)

speed = 32000

while True:
    print("l")
    PWMA.duty_u16(speed)
    DIRA.duty_u16(0)
    time.sleep(1)
    PWMA.duty_u16(0)
    DIRA.duty_u16(0)
    time.sleep(1)
    PWMA.duty_u16(0)
    DIRA.duty_u16(speed)
    time.sleep(1)
    PWMA.duty_u16(0)
    DIRA.duty_u16(0)
    time.sleep(1)
      
'''