From d20a117aa8c3a452fecfd8499cc93bd4858224b4 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Tue, 23 Jun 2026 23:13:40 -0700 Subject: [PATCH 1/3] add pivot to face point maneuver --- ateam_msgs/msg/RobotMotionCommand.msg | 9 ++++++- motion/ateam_controls/controls | 2 +- .../src/radio_bridge_node.cpp | 25 ++++++++++++++++--- radio/ateam_radio_msgs/software-communication | 2 +- 4 files changed, 31 insertions(+), 7 deletions(-) diff --git a/ateam_msgs/msg/RobotMotionCommand.msg b/ateam_msgs/msg/RobotMotionCommand.msg index 0b66fc9a..b7eaa9b2 100644 --- a/ateam_msgs/msg/RobotMotionCommand.msg +++ b/ateam_msgs/msg/RobotMotionCommand.msg @@ -10,6 +10,8 @@ float64 limit_acc_angular # (rad/s/s) # pivot parameters float64 pivot_orbit_radius # (m) float64 pivot_inset_angle # (rad) +uint8 pivot_direction # orbit drive direction: PD_FORWARD=0, PD_BACKWARD=1 +bool pivot_compute_inset_angle # if true, firmware derives inset_angle from peak angular vel uint8 kick_request float32 kick_speed # (m/s) @@ -25,6 +27,10 @@ uint8 KR_CHIP_NOW = 5 uint8 KR_CHIP_TOUCH = 6 uint8 KR_CHIP_CAPTURED = 7 +# pivot direction (orbit drive direction) +uint8 PD_FORWARD = 0 +uint8 PD_BACKWARD = 1 + # body control modes uint8 BCM_OFF = 0 uint8 BCM_GLOBAL_POSITION = 1 @@ -32,4 +38,5 @@ uint8 BCM_GLOBAL_VELOCITY = 2 uint8 BCM_LOCAL_VELOCITY = 3 uint8 BCM_GLOBAL_ACCEL = 4 uint8 BCM_LOCAL_ACCEL = 5 -uint8 BCM_PIVOT = 6 +uint8 BCM_HEADING_PIVOT = 6 +uint8 BCM_POINT_PIVOT = 7 diff --git a/motion/ateam_controls/controls b/motion/ateam_controls/controls index 2adc62d7..70266877 160000 --- a/motion/ateam_controls/controls +++ b/motion/ateam_controls/controls @@ -1 +1 @@ -Subproject commit 2adc62d7f057e7551c359f21b213f716710eb4e3 +Subproject commit 70266877ce782a864fe084801bee2eacbe56faca diff --git a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp index 9e613fbc..ec264b0e 100644 --- a/radio/ateam_radio_bridge/src/radio_bridge_node.cpp +++ b/radio/ateam_radio_bridge/src/radio_bridge_node.cpp @@ -366,14 +366,31 @@ class RadioBridgeNode : public rclcpp::Node static_cast(command.acceleration.theta), }; break; - case ateam_msgs::msg::RobotMotionCommand::BCM_PIVOT: - control_msg.body_control_mode = BCM_PIVOT; - control_msg.cmd.pivot = { + case ateam_msgs::msg::RobotMotionCommand::BCM_HEADING_PIVOT: + control_msg.body_control_mode = BCM_HEADING_PIVOT; + control_msg.cmd.heading_pivot = { static_cast(command.pose.theta), static_cast(command.limit_vel_angular), static_cast(command.limit_acc_angular), static_cast(command.pivot_orbit_radius), - static_cast(command.pivot_inset_angle) + static_cast(command.pivot_inset_angle), + static_cast(command.pivot_direction), + static_cast(command.pivot_compute_inset_angle), + {0, 0} + }; + break; + case ateam_msgs::msg::RobotMotionCommand::BCM_POINT_PIVOT: + control_msg.body_control_mode = BCM_POINT_PIVOT; + control_msg.cmd.point_pivot = { + static_cast(command.pose.x), + static_cast(command.pose.y), + static_cast(command.limit_vel_angular), + static_cast(command.limit_acc_angular), + static_cast(command.pivot_orbit_radius), + static_cast(command.pivot_inset_angle), + static_cast(command.pivot_direction), + static_cast(command.pivot_compute_inset_angle), + {0, 0} }; break; default: diff --git a/radio/ateam_radio_msgs/software-communication b/radio/ateam_radio_msgs/software-communication index 5b0c64fa..5c834d00 160000 --- a/radio/ateam_radio_msgs/software-communication +++ b/radio/ateam_radio_msgs/software-communication @@ -1 +1 @@ -Subproject commit 5b0c64fabb26156ae38a9e415f3472e65f3324c4 +Subproject commit 5c834d0091c4201d194662594233ea70714649e8 From 3749b96be1016f608e126581c089f091a0cee3e5 Mon Sep 17 00:00:00 2001 From: Matthew Woodward Date: Wed, 24 Jun 2026 17:08:13 -0400 Subject: [PATCH 2/3] Update CMakeLists for new pivot messages --- radio/ateam_radio_msgs/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/radio/ateam_radio_msgs/CMakeLists.txt b/radio/ateam_radio_msgs/CMakeLists.txt index e7703b97..91bb1b66 100644 --- a/radio/ateam_radio_msgs/CMakeLists.txt +++ b/radio/ateam_radio_msgs/CMakeLists.txt @@ -23,7 +23,8 @@ set(RADIO_STRUCTS_TO_GENERATE LocalVelocityCommand GlobalAccelerationCommand LocalAccelerationCommand - PivotCommand + PointPivotCommand + HeadingPivotCommand BasicTelemetry ErrorTelemetry ExtendedTelemetry From 412ce1f49b755a61e1a59c8e923df3c1b16558f8 Mon Sep 17 00:00:00 2001 From: Matthew Woodward Date: Wed, 24 Jun 2026 18:05:49 -0400 Subject: [PATCH 3/3] Update submodules --- motion/ateam_controls/controls | 2 +- radio/ateam_radio_msgs/software-communication | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/motion/ateam_controls/controls b/motion/ateam_controls/controls index 70266877..5f72604c 160000 --- a/motion/ateam_controls/controls +++ b/motion/ateam_controls/controls @@ -1 +1 @@ -Subproject commit 70266877ce782a864fe084801bee2eacbe56faca +Subproject commit 5f72604ca70a3a28bed8b7fc4125f798e652656f diff --git a/radio/ateam_radio_msgs/software-communication b/radio/ateam_radio_msgs/software-communication index 5c834d00..8b77146d 160000 --- a/radio/ateam_radio_msgs/software-communication +++ b/radio/ateam_radio_msgs/software-communication @@ -1 +1 @@ -Subproject commit 5c834d0091c4201d194662594233ea70714649e8 +Subproject commit 8b77146da2112944074d343e9b5d57cad65e5c0d