wip: find turning bug

zauberzeug · Jul 30, 2024 · 45dbff6 · 45dbff6
1 parent 04db3a1
commit 45dbff6
Showing 1 changed file with 10 additions and 2 deletions.
diff --git a/tests/test_navigation.py b/tests/test_navigation.py
@@ -28,9 +28,17 @@ async def test_straight_line_with_high_angles(system: System):
     assert isinstance(system.field_friend.wheels, rosys.hardware.WheelsSimulation)
     predicted_yaw = 190
     start_yaw = system.odometer.prediction.yaw
-    target_yaw = start_yaw + np.deg2rad(predicted_yaw)
+    target_yaw = 0.0 + np.deg2rad(predicted_yaw)
     await system.driver.wheels.drive(*system.driver._throttle(0, 0.1))  # pylint: disable=protected-access
-    await forward(until=lambda: abs(rosys.helpers.angle(system.odometer.prediction.yaw, target_yaw)) < np.deg2rad(0.01))
+    while True:
+        yaw_diff = rosys.helpers.angle(system.odometer.prediction.yaw, target_yaw)
+        stop = abs(yaw_diff) < np.deg2rad(0.05)
+        if system.odometer.prediction.yaw_deg > 170:
+            await forward(0.1)
+        await forward(0.1)
+        if stop:
+            break
+    # await forward(until=lambda: abs(rosys.helpers.angle(system.odometer.prediction.yaw, target_yaw)) < np.deg2rad(0.05), timeout=300)
     await system.driver.wheels.stop()
     assert isinstance(system.current_navigation, StraightLineNavigation)
     system.current_navigation.length = 1.0