From 9205705d9603bac24dd705ee991346c9d7d610de Mon Sep 17 00:00:00 2001
From: leo <leo@latentspace.cloud>
Date: Sun, 28 Sep 2025 18:34:13 +0200
Subject: [PATCH] programma facile

---
 leo/giro.py  | 44 ++++++++++++++++++++++++++++++++++++++++++++
 leo/ruote.py | 29 ++++++++++++++++++-----------
 2 files changed, 62 insertions(+), 11 deletions(-)
 create mode 100644 leo/giro.py

diff --git a/leo/giro.py b/leo/giro.py
new file mode 100644
index 0000000..0b7e3d8
--- /dev/null
+++ b/leo/giro.py
@@ -0,0 +1,44 @@
+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))
+
+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, 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.rotation(Axis.Z)
+        print(f"gradi: {gradi}")
+        speed = g - gradi
+        if g < 0:
+            mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False)
+            mDestra.run_angle(speed, g, then=Stop.NONE, wait=True)
+        elif g > 0:
+            mSinistra.run_angle(speed, g, then=Stop.NONE, wait=False)
+            mDestra.run_angle(speed, g, then=Stop.NONE, wait=True)
+
+        if gradi == g:
+            break
+
+
+# reset_yaw()
+print("G: " + str(hub.imu.rotation(Axis.Z)))
+precision_turn(90)
diff --git a/leo/ruote.py b/leo/ruote.py
index 7de777c..da39bc1 100644
--- a/leo/ruote.py
+++ b/leo/ruote.py
@@ -1,12 +1,13 @@
 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.tools import vector
 
 # from pybricks.hub import PrimHub
 
 hub = InventorHub(top_side=vector(1, 1, 0), front_side=vector(-1, 1, 0))
-mDestra = Motor(Port.F)
+gradi = Axis.Z
+mDestra = Motor(Port.A)
 mSinistra = Motor(Port.B)
 
 
@@ -25,7 +26,7 @@ def turn(speed_L, speed_R, gradi):
 # reset_angle()
 
 # vai_avanti(1000, 360)
-turn(1000, 1000, 120)
+# turn(1000, 1000, 120)
 # vai_avanti(1000, 360)
 # turn(1000, 1000, 360)
 # vai_avanti(1000, 360)
@@ -33,12 +34,18 @@ turn(1000, 1000, 120)
 # 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)
+# giroX = hub.imu.rotation(Axis.X)
+# giroY = hub.imu.rotation(Axis.Y)
+# giroZ = hub.imu.rotation(Axis.Z)
+# print(giroX)
+# print(giroY)
+# print(giroZ)
 
-while not (mSinistra.done() and mDestra.done()):
-    wait(10)
+# 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)