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.parameters import Axis, Port, Stop
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)
mSinistra = Motor(Port.B)
@ -25,20 +25,21 @@ def vai_avanti(speed, gradi):
def precision_turn(g):
while True:
gradi = hub.imu.rotation(Axis.Z)
gradi = hub.imu.heading()
print(f"gradi: {gradi}")
speed = g - gradi
print(speed)
if g < 0:
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:
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
# reset_yaw()
hub.imu.reset_heading(0)
print("G: " + str(hub.imu.rotation(Axis.Z)))
precision_turn(90)
# precision_turn(90)