51 lines
1.2 KiB
Python
51 lines
1.2 KiB
Python
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
|
|
|
|
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_L, speed_R, gradi):
|
|
print("girogiro")
|
|
mSinistra.run_angle(speed_L, gradi, then=Stop.NONE, wait=False)
|
|
mDestra.run_angle(speed_R, 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(-2000, 6666)
|