from machine import Pin, PWM
import time

#Speed variable (tweak this to the lowest possible, that still starts the motors)
speed=25000

### MOTORS
#Pin Setup (left motor)
pwm1 = PWM(Pin(22))
in1  = Pin(21, Pin.OUT)
in2  = Pin(20, Pin.OUT)

#Pin Setup (right motor)
in3  = Pin(19, Pin.OUT)
in4  = Pin(18, Pin.OUT)
pwm2 = PWM(Pin(17))

### SENSORS
#IR Sensors
sensorl=Pin(0, Pin.IN, Pin.PULL_UP)
sensorr=Pin(1, Pin.IN, Pin.PULL_UP)

#Set PWM freqency for PWM-Pins
pwm1.freq(1000)
pwm2.freq(1000)

def forwards():
    in1.value(0)
    in2.value(1)
    in3.value(0)
    in4.value(1)
    pwm1.duty_u16(speed)
    pwm2.duty_u16(speed)
    
def left():
    in1.value(0)
    in2.value(1)
    in3.value(0)
    in4.value(0)
    pwm1.duty_u16(speed)
    pwm2.duty_u16(speed)
    
def right():
    in1.value(0)
    in2.value(0)
    in3.value(0)
    in4.value(1)
    pwm1.duty_u16(speed)
    pwm2.duty_u16(speed)
    
def stop():
    in1.value(0)
    in2.value(0)
    in3.value(0)
    in4.value(0)
    pwm1.duty_u16(0)
    pwm2.duty_u16(0)
    #print("Stop")

# Main loop
last_values=(1,1)
while True:
    # Check if the sensor reading has changed (to keep this loop efficient)
    if last_values != (sensorl.value(), sensorr.value()):
        
        if sensorl.value() == 0 and sensorr.value() == 0:
            forwards()
        elif sensorl.value() == 1 and sensorr.value() == 0:
            left()
        elif sensorl.value() == 0 and sensorr.value() == 1:
            right()
        else:
            stop()
        last_values = (sensorl.value(), sensorr.value())

    time.sleep_ms(5)