from pybricks.hubs import PrimeHub
from pybricks.parameters import Axis, Direction, Port
from pybricks.pupdevices import Motor
from pybricks.robotics import DriveBase
from pybricks.tools import wait

import batteria
from assi import A

left = Motor(Port.B, Direction.COUNTERCLOCKWISE)
right = Motor(Port.A)


hub = PrimeHub(top_side=A.UP, front_side=A.BACKWARD)
db = DriveBase(left, right, wheel_diameter=56, axle_track=105)
db.use_gyro(True)
# (mm/s, mm/s², deg/s, deg/s²)
db.settings(100, 100, 90, 50)
wait(300)
db.reset(angle=0)


db.straight(100)
db.turn(90)
db.straight(100)
db.turn(90)
db.straight(100)
db.turn(90)
db.straight(100)
db.turn(90)

print(f"drived {db.distance()}")