From ebd07258f0a7b3a191949daf3955c7ee8d408c51 Mon Sep 17 00:00:00 2001 From: David Date: Thu, 9 Oct 2025 16:19:34 -0500 Subject: [PATCH] refactor: replace speed_mode with max_duty for core control Makes it make more sense --- src/core_pkg/core_pkg/core_node.py | 31 ++++++++++----------------- src/headless_pkg/src/headless_node.py | 18 ++++++++-------- src/ros2_interfaces_pkg | 2 +- 3 files changed, 21 insertions(+), 30 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 8e97180..69977a4 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -14,6 +14,7 @@ import sys import threading import glob from scipy.spatial.transform import Rotation +from math import copysign from std_msgs.msg import String, Header from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus @@ -29,10 +30,6 @@ CORE_WHEELBASE = 0.836 # meters CORE_WHEEL_RADIUS = 0.171 # meters CORE_GEAR_RATIO = 100 # Clucky: 100:1, Testbed: 64:1 -WALKING_SPEED = 0.5 # duty cycle -FAST_SPEED = 0.9 # duty cycle -SLOW_SPEED = 0.25 # duty cycle - control_qos = qos.QoSProfile( history=qos.QoSHistoryPolicy.KEEP_LAST, depth=2, @@ -73,9 +70,9 @@ class SerialRelay(Node): self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, qos_profile=control_qos) # manual twist -- [-1, 1] rather than real units self.twist_man_sub_ = self.create_subscription(Twist, '/core/twist', self.twist_man_callback, qos_profile=control_qos) - # manual flags -- brake mode and speed mode + # manual flags -- brake mode and max duty cycle self.control_state_sub_ = self.create_subscription(CoreCtrlState, '/core/control/state', self.control_state_callback, qos_profile=control_qos) - self.twist_speed_mode = 0 # -1 = slow, 0 = walk, 1 = fast + self.twist_max_duty = 0.5 # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5 # Feedback @@ -248,8 +245,8 @@ 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 - linear = 0.25 * (linear / abs(linear)) - angular = 0.25 * (angular / abs(angular)) + linear = copysign(0.25, linear) + angular = copysign(0.25, angular) duty_left = linear - angular duty_right = linear + angular @@ -257,16 +254,10 @@ class SerialRelay(Node): duty_left /= scale duty_right /= scale - # Apply speed mode - if self.twist_speed_mode == -1: # slow - duty_left = map_range(duty_left, -1, 1, -SLOW_SPEED, SLOW_SPEED) - duty_right = map_range(duty_right, -1, 1, -SLOW_SPEED, SLOW_SPEED) - elif self.twist_speed_mode == 0: # walk - duty_left = map_range(duty_left, -1, 1, -WALKING_SPEED, WALKING_SPEED) - duty_right = map_range(duty_right, -1, 1, -WALKING_SPEED, WALKING_SPEED) - elif self.twist_speed_mode == 1: # fast - duty_left = map_range(duty_left, -1, 1, -FAST_SPEED, FAST_SPEED) - duty_right = map_range(duty_right, -1, 1, -FAST_SPEED, FAST_SPEED) + # Apply max duty cycle + # Joysticks provide values [-1, 1] rather than real units + duty_left = map_range(duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty) + duty_right = map_range(duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty) self.send_viccan(19, [duty_left, duty_right]) @@ -274,8 +265,8 @@ class SerialRelay(Node): # Brake mode self.send_viccan(18, [msg.brake_mode]) - # Speed mode - self.twist_speed_mode = msg.speed_mode # twist_man_callback will handle this + # Max duty cycle + self.twist_max_duty = msg.max_duty # twist_man_callback will handle this def send_cmd(self, msg: str): if self.launch_mode == 'anchor': diff --git a/src/headless_pkg/src/headless_node.py b/src/headless_pkg/src/headless_node.py index df549f5..fd23a8c 100755 --- a/src/headless_pkg/src/headless_node.py +++ b/src/headless_pkg/src/headless_node.py @@ -79,7 +79,7 @@ class Headless(Node): self.ctrl_mode = "core" # Start in core mode self.core_brake_mode = False - self.core_speed_mode = 0 # -1 = slow, 0 = walk, 1 = fast + self.core_max_duty = 0.5 # Default max duty cycle (walking speed) # Rumble when node is ready (returns False if rumble not supported) self.gamepad.rumble(0.7, 0.8, 150) @@ -183,23 +183,23 @@ class Headless(Node): # Brake mode new_brake_mode = button_a - # Speed mode + # Max duty cycle if left_bumper: - new_speed_mode = -1 + new_max_duty = 0.25 elif right_bumper: - new_speed_mode = 1 + new_max_duty = 0.9 else: - new_speed_mode = 0 + new_max_duty = 0.5 # Only publish if needed - if new_brake_mode != self.core_brake_mode or new_speed_mode != self.core_speed_mode: + if new_brake_mode != self.core_brake_mode or new_max_duty != self.core_max_duty: self.core_brake_mode = new_brake_mode - self.core_speed_mode = new_speed_mode + self.core_max_duty = new_max_duty state_msg = CoreCtrlState() state_msg.brake_mode = bool(self.core_brake_mode) - state_msg.speed_mode = int(self.core_speed_mode) + state_msg.max_duty = float(self.core_max_duty) self.core_state_pub_.publish(state_msg) - self.get_logger().info(f"[Core State] Brake: {self.core_brake_mode}, Speed: {self.core_speed_mode}") + self.get_logger().info(f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}") # ARM and BIO diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index bb04ec4..f7276a2 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit bb04ec4a176fead136bdc3031a9d08efd5cf068e +Subproject commit f7276a2c272ed94b9ce1254ee79d79b81a52832a