✨

2022-09-28 14:06:13 +02:00
parent 95c011e4e7
commit 7fb14f9345
1 changed files with 2 additions and 1 deletions
--- a/drive_between_arucos.py
+++ b/drive_between_arucos.py
@ -74,9 +74,10 @@ def main():
        np.average(aruco_positions[:,0]),
        np.average(aruco_positions[:,1])
    ]
+    print(position)

    angle = np.rad2deg(np.arctan(position[1]/position[0]))
-    drive_distance = np.sqrt(position[0]**2 + position[1]**2)
+    drive_distance = np.sqrt(position[0]**2 + position[1]**2) + 22.5
    if angle < 0:
        arlo.go_diff(POWER, POWER, 0, 1)
        sleep((abs(angle) * TURN_T)/1000)