from pybricks.parameters import Port, Stop
from pybricks.pupdevices import Motor
from pybricks.tools import wait

mDestra = Motor(Port.F)
mSinistra = Motor(Port.B)


def vai_avanti(speed, gradi):
    mSinistra.run_angle(speed, gradi, then=Stop.NONE, wait=False)
    mDestra.run_angle(-speed, gradi, then=Stop.NONE, wait=False)


def turn(speed_L, speed_R, gradi):
    mSinistra.run_angle(speed_L, gradi, then=Stop.NONE, wait=False)
    mDestra.run_angle(speed_R, gradi, then=Stop.NONE, wait=False)


print("test1")
vai_avanti(1000, 360)
print("test2")
turn(1000, 1000, 360)
print("test3")
vai_avanti(1000, 360)
turn(1000, 1000, 360)


while not (mSinistra.done() and mDestra.done()):
    wait(10)