47 lines
1.4 KiB
Python
47 lines
1.4 KiB
Python
from pybricks.hub import PrimHub
|
|
from pybricks.hubs import InventorHub
|
|
from pybricks.parameters import Axis, Port, Stop
|
|
from pybricks.pupdevices import Motor
|
|
from pybricks.tools import vector
|
|
|
|
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)
|
|
|
|
|
|
def vai_avanti(speed: int, gradi: int):
|
|
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_L, speed_R, g):
|
|
print("girogiro")
|
|
mSinistra.run_angle(speed_L, g, then=Stop.NONE, wait=False)
|
|
mDestra.run_angle(speed_R, g, then=Stop.NONE, wait=True)
|
|
|
|
|
|
def precision_turn(g):
|
|
while True:
|
|
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=False)
|
|
elif g > 0:
|
|
mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False)
|
|
mDestra.run_angle(speed, g, then=Stop.NONE, wait=False)
|
|
|
|
if speed < 1 and speed > -1:
|
|
break
|
|
|
|
|
|
hub.imu.reset_heading(0)
|
|
a = hub.imu.heading()
|
|
# print("G: " + str(hub.imu.rotation(Axis.Z)))
|
|
print(a)
|
|
precision_turn(90)
|