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 giorobot 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, 100, 50)
# max mm/s = 864
# mm/s quadrato = accelerazione
#
wait(300)
db.reset(angle=0)


db.straight(400)
db.straight(-100)