aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

fix: correct turning direction

Browse Source
This commit is contained in:
David
2025-09-08 23:01:40 -05:00
parent 0c1a368499
commit bac9b3cbe0
1 changed files with 7 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/core_pkg/core_pkg/core_node.py
View File

@@ -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)