from machine import Pin, PWM
import time

# Motor A connections
enA = PWM(Pin(18))
in1 = Pin(4, Pin.OUT)
in2 = Pin(17, Pin.OUT)

# Motor B connections
enB = PWM(Pin(12))
in3 = Pin(27, Pin.OUT)
in4 = Pin(22, Pin.OUT)

# PWM frequency
enA.freq(1000)
enB.freq(1000)

def set_speed(motor_pwm, speed):
    # speed: 0 to 100
    duty = int(speed * 65535 / 100)
    motor_pwm.duty_u16(duty)

def forward(speed):
    print("Forward")

    set_speed(enA, speed)
    set_speed(enB, speed)

    in1.value(1)
    in2.value(0)

    in3.value(1)
    in4.value(0)

def backward(speed):
    print("Backward")

    set_speed(enA, speed)
    set_speed(enB, speed)

    in1.value(0)
    in2.value(1)

    in3.value(0)
    in4.value(1)

def left(speed):
    print("Left")

    set_speed(enA, speed)
    set_speed(enB, speed)

    in1.value(0)
    in2.value(1)

    in3.value(1)
    in4.value(0)

def right(speed):
    print("Right")

    set_speed(enA, speed)
    set_speed(enB, speed)

    in1.value(1)
    in2.value(0)

    in3.value(0)
    in4.value(1)

def stop():
    print("Stop")

    set_speed(enA, 0)
    set_speed(enB, 0)

    in1.value(0)
    in2.value(0)
    in3.value(0)
    in4.value(0)

def directionControl():
    print("Direction control test")

    forward(100)
    time.sleep(2)

    backward(100)
    time.sleep(2)

    stop()

def speedControl():
    print("Speed control test")

    in1.value(1)
    in2.value(0)

    in3.value(1)
    in4.value(0)

    print("Accelerating")

    for speed in range(0, 101):
        set_speed(enA, speed)
        set_speed(enB, speed)
        time.sleep(0.02)

    print("Decelerating")

    for speed in range(100, -1, -1):
        set_speed(enA, speed)
        set_speed(enB, speed)
        time.sleep(0.02)

    stop()

stop()

while True:
    directionControl()
    time.sleep(1)

    speedControl()
    time.sleep(1)