From 21d20a3cec3e83bae55579e8935570adb07b562d Mon Sep 17 00:00:00 2001 From: Rodja Trappe Date: Wed, 24 Jul 2024 08:01:59 +0200 Subject: [PATCH] Fix yaw computation (#134) * fixes for presenting f12 with coins * fix yaw order * revert docker container change * revert tornado height --------- Co-authored-by: Lukas Baecker Co-authored-by: Pascal Schade --- field_friend/automations/navigation/navigation.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/field_friend/automations/navigation/navigation.py b/field_friend/automations/navigation/navigation.py index 388b6cf3..7660b3be 100644 --- a/field_friend/automations/navigation/navigation.py +++ b/field_friend/automations/navigation/navigation.py @@ -4,6 +4,7 @@ from typing import TYPE_CHECKING, Any import rosys +from rosys.helpers import angle from nicegui import ui from ..implements import Implement @@ -83,7 +84,7 @@ async def _drive(self, distance: float) -> None: async def _drive_to_yaw(self, distance: float, yaw: float): deadline = rosys.time() + 2 start_position = self.odometer.prediction.point - yaw -= self.odometer.prediction.yaw # take current yaw into account and only steer the difference + yaw = angle(self.odometer.prediction.yaw, yaw) # take current yaw into account and only steer the difference with self.driver.parameters.set(linear_speed_limit=self.linear_speed_limit, angular_speed_limit=self.angular_speed_limit): await self.driver.wheels.drive(*self.driver._throttle(1, yaw)) # pylint: disable=protected-access try: