from machine import Pin, PWM, ADC
import time, utime
led = Pin(25, Pin.OUT)
led2 = Pin(20, Pin.OUT)
lmotordir = Pin(7, Pin.OUT)
rmotordir = Pin(8, Pin.OUT)
lmotorpwm = PWM(Pin(9))
rmotorpwm = PWM(Pin(10))
rfront = ADC(Pin(26)) # right front sensor (was A1 on Nano)
lfront = ADC(Pin(27)) # left front sensor (was A2 on Nano)
lmotorpwm.freq(1000) # set PWM frequency
rmotorpwm.freq(1000) # set PWM frequency
rduty = 0 # in range 0 to 65025
lduty = 0 # in range 0 to 65025
rmotorpwm.duty_u16(rduty)
lmotorpwm.duty_u16(lduty)

led.on()
lmotordir.off() # sets pin high to go forward
rmotordir.off() # sets pin high to go forward
rmotorpwm.duty_u16(0) # set right speed to zero
lmotorpwm.duty_u16(0) # set left speed to zero
efactor = 2
basespeed = 20000 # range 0 up to 65025


while True:

    lsense = lfront.read_u16() # read left front sensor
    rsense = rfront.read_u16() # read right front sensor
    lspeed = basespeed
    rspeed = basespeed   
    error = efactor * (lsense - rsense)
    #print(str(lsense) + " " + str(rsense) + " " + str(error))
    lspeed = lspeed + error
    rspeed = rspeed - error
    if error > 0 :
        led.off()
    if error < 0 :
        led.on()
    #lspeed = 0
    #rspeed = 0
    
    rmotorpwm.duty_u16(rspeed) # set right speed
    lmotorpwm.duty_u16(lspeed) # set left speed
    time.sleep(0.001)