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
from robots import leorobot as robot

hub = robot.hub
db = robot.db


def gira_fino_a_quando(gradi: int):
    posizione_iniziale = db.angle()
    gradi_mancanti = gradi - posizione_iniziale
    db.turn(gradi_mancanti)
    # dangolo = db.angle()
    # if angolo < gradi:
    # db.turn(gradi - angolo)


db.use_gyro(True)  # abilitiamo il giroscopio
# (mm/s, mm/s², deg/s, deg/s²)
db.settings(100, 100, 90, 50)
wait(300)
db.reset(angle=0)

db.straight(100)
gira_fino_a_quando(90)
print(db.angle())
db.straight(100)
gira_fino_a_quando(180)
print(db.angle())
db.straight(100)
gira_fino_a_quando(270)
print(db.angle())
db.straight(100)
gira_fino_a_quando(360)
print(db.angle())


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