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..5f72604c 160000 --- a/motion/ateam_controls/controls +++ b/motion/ateam_controls/controls @@ -1 +1 @@ -Subproject commit 2adc62d7f057e7551c359f21b213f716710eb4e3 +Subproject commit 5f72604ca70a3a28bed8b7fc4125f798e652656f 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/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 diff --git a/radio/ateam_radio_msgs/software-communication b/radio/ateam_radio_msgs/software-communication index 5b0c64fa..8b77146d 160000 --- a/radio/ateam_radio_msgs/software-communication +++ b/radio/ateam_radio_msgs/software-communication @@ -1 +1 @@ -Subproject commit 5b0c64fabb26156ae38a9e415f3472e65f3324c4 +Subproject commit 8b77146da2112944074d343e9b5d57cad65e5c0d