FLL/leo/ruote.py
2025-09-28 19:01:12 +02:00

53 lines
1.3 KiB
Python

from pybricks.hubs import InventorHub
from pybricks.parameters import Axis, Port, Stop
from pybricks.pupdevices import Motor
from pybricks.tools import vector, wait
# from pybricks.hub import PrimHub
hub = InventorHub(top_side=vector(1, 1, 0), front_side=vector(-1, 1, 0))
gradi = Axis.Z
mDestra = Motor(Port.A)
mSinistra = Motor(Port.B)
def vai_avanti(speed, gradi):
print(f"vado avanti di {gradi} a {speed}")
mSinistra.run_angle(speed, gradi, then=Stop.NONE, wait=False)
mDestra.run_angle(-speed, gradi, then=Stop.NONE, wait=True)
def turn(speed, gradi):
print("girogiro")
mSinistra.run_angle(speed, gradi, then=Stop.NONE, wait=False)
mDestra.run_angle(speed, gradi, then=Stop.NONE, wait=True)
# reset_angle()
# vai_avanti(1000, 360)
# turn(1000, 1000, 120)
# vai_avanti(1000, 360)
# turn(1000, 1000, 360)
# vai_avanti(1000, 360)
# turn(1000, 1000, 360)
# vai_avanti(1000, 360)
# turn(1000, 1000, 360)
# giroX = hub.imu.rotation(Axis.X)
# giroY = hub.imu.rotation(Axis.Y)
# giroZ = hub.imu.rotation(Axis.Z)
# print(giroX)
# print(giroY)
# print(giroZ)
# start_angle_x = hub.imu.rotation(Axis.X)
# start_angle_y = hub.imu.rotation(Axis.Y)
# while not (mSinistra.done() and mDestra.done()):
# wait(10)
# vai_avanti(-1000, 1090.17)
turn(-500, 180)
wait(2000)
turn(-500, 180)