From bac9b3cbe094a188a1f8da24b5b04b47ba6bbeed Mon Sep 17 00:00:00 2001 From: David Date: Mon, 8 Sep 2025 23:01:40 -0500 Subject: [PATCH] fix: correct turning direction --- src/core_pkg/core_pkg/core_node.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 517cfe9..513a41b 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -217,7 +217,7 @@ class SerialRelay(Node): def cmd_vel_callback(self, msg: TwistStamped): linear = msg.twist.linear.x - angular = msg.twist.angular.z + angular = -msg.twist.angular.z vel_left_rads = (linear - (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS vel_right_rads = (linear + (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS @@ -235,13 +235,18 @@ class SerialRelay(Node): if abs(linear) > 1 or abs(angular) > 1: # if speed is greater than 1, then there is a problem # make it look like a problem and don't just run away lmao - drive_speed = 0.25 + linear = 0.25 * (linear / abs(linear)) + angular = 0.25 * (angular / abs(angular)) duty_left = linear - angular duty_right = linear + angular scale = max(1, abs(duty_left), abs(duty_right)) duty_left /= scale duty_right /= scale + + if (linear < 0): # reverse direction when going backwards + duty_left *= -1 + duty_right *= -1 command = f"can_relay_tovic,core,19,{duty_left},{duty_right}\n" self.send_cmd(command)