senpai/FLL
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

tentativo di aggiustare il giroscopio

Browse source
This commit is contained in:
leo 2025-09-28 20:02:19 +02:00
parent fae6a3cf15
commit f4a83747c2
1 changed files with 10 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
leo/giro.py
View file

@ -1,11 +1,11 @@
from pybricks.hubs import InventorHub from pybricks.hubs import InventorHub
from pybricks.parameters import Axis, Port, Stop from pybricks.parameters import Axis, Port, Stop
from pybricks.pupdevices import Motor from pybricks.pupdevices import Motor
from pybricks.tools import vector
# from pybricks.hub import PrimHub ##from pybricks.hub import PrimHub
hub = InventorHub(top_side=vector(1, 1, 0), front_side=vector(-1, 1, 0)) # hub = InventorHub(top_side=vector(0, 0, 1), front_side=vector(0, 1, 0))
hub = InventorHub(top_side=Axis.Z, front_side=Axis.Y)
mDestra = Motor(Port.A) mDestra = Motor(Port.A)
mSinistra = Motor(Port.B) mSinistra = Motor(Port.B)
@ -25,20 +25,21 @@ def vai_avanti(speed, gradi):
def precision_turn(g): def precision_turn(g):
while True: while True:
gradi = hub.imu.rotation(Axis.Z) gradi = hub.imu.heading()
print(f"gradi: {gradi}") print(f"gradi: {gradi}")
speed = g - gradi speed = g - gradi
print(speed)
if g < 0: if g < 0:
mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False) mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False)
mDestra.run_angle(speed, g, then=Stop.NONE, wait=True) mDestra.run_angle(speed, g, then=Stop.NONE, wait=False)
elif g > 0: elif g > 0:
mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False) mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False)
mDestra.run_angle(speed, g, then=Stop.NONE, wait=True) mDestra.run_angle(speed, g, then=Stop.NONE, wait=False)
if gradi == g: if speed < 1 and speed > -1:
break break
# reset_yaw() hub.imu.reset_heading(0)
print("G: " + str(hub.imu.rotation(Axis.Z))) print("G: " + str(hub.imu.rotation(Axis.Z)))
precision_turn(90) # precision_turn(90)