From 9d3daf70fed82de642d1448941c4b6c0c5b978a3 Mon Sep 17 00:00:00 2001 From: cxh Date: Fri, 6 Jun 2025 16:10:18 +0800 Subject: [PATCH] Auto commit at 2025-06-06 16:10:18 --- src/radio_ctrl/src/radio_ctrl.cpp | 59 ++++++++++++++++--------------- 1 file changed, 30 insertions(+), 29 deletions(-) diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index a0225ff..fdd7fe4 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -8,6 +8,7 @@ constexpr float EPS_GEAR_RATIO = 16.5f; constexpr float DELTA_T = 0.02f; // 20ms +const uint16_t speed[3] = {192, 992, 1792}; class SBUSNode : public rclcpp::Node { @@ -71,9 +72,9 @@ private: for (int i = 0; i < 10; ++i) { ch_data[i] = uart_handler_->get_channel_value(i); - printf("ch[%d]:%d ", i, ch_data[i]); + // printf("ch[%d]:%d ", i, ch_data[i]); } - printf("\n"); + // printf("\n"); if (ch_data[6] == 192) // 是否使能车辆控制 { @@ -98,17 +99,17 @@ private: } // 油门 / 刹车逻辑 - if (ch_data[1] >= 1016) + if (ch_data[1] <= speed[1]) { msg.brake = 1; - msg.brake_time_ms = static_cast(30000 + (500 - 30000) * (ch_data[1] - 1016) / (1816 - 1016)); + msg.brake_time_ms = static_cast(30000 + (500 - 30000) * (speed[1] - ch_data[1]) / (speed[1] - speed[0])); msg.rpm = 0; } else { msg.brake = 0; msg.brake_time_ms = 500; - msg.rpm = static_cast(MCU_RPM_MAX * (1016 - ch_data[1]) / (1016 - 216)); + msg.rpm = static_cast(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1])); } // 一键清扫 @@ -138,7 +139,7 @@ private: } // 转向逻辑 - float target_angle = (static_cast(ch_data[3]) - 800) / 800 * EPS_ANGLE_MAX; + float target_angle = (992 - static_cast(ch_data[3])) / 800 * EPS_ANGLE_MAX; // 角速度（度/秒） float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T; @@ -177,29 +178,29 @@ private: // 发布控制消息 pub_->publish(msg); - // RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" - // << "\n mcu_enabled: " << msg.mcu_enabled - // << "\n brake: " << static_cast(msg.brake) - // << "\n gear: " << static_cast(msg.gear) - // << "\n rpm: " << msg.rpm - // << "\n brake_time_ms: " << msg.brake_time_ms - // << "\n angle: " << msg.angle - // << "\n angle_speed: " << msg.angle_speed - // << "\n main_brush_lift: " << msg.main_brush_lift - // << "\n edge_brush_lift: " << msg.edge_brush_lift - // << "\n vacuum: " << msg.vacuum - // << "\n spray: " << msg.spray - // << "\n mud_flap: " << msg.mud_flap - // << "\n dust_shake: " << msg.dust_shake - // << "\n left_light: " << msg.left_light - // << "\n right_light: " << msg.right_light - // << "\n night_light: " << msg.night_light - // << "\n brake_light: " << msg.brake_light - // << "\n headlight: " << msg.headlight - // << "\n main_brush_spin: " << msg.main_brush_spin - // << "\n edge_brush_spin: " << msg.edge_brush_spin - // << "\n main_brush_pwm: " << static_cast(msg.main_brush_pwm) - // << "\n edge_brush_pwm: " << static_cast(msg.edge_brush_pwm)); + RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" + << "\n mcu_enabled: " << msg.mcu_enabled + << "\n brake: " << static_cast(msg.brake) + << "\n gear: " << static_cast(msg.gear) + << "\n rpm: " << msg.rpm + << "\n brake_time_ms: " << msg.brake_time_ms + << "\n angle: " << msg.angle + << "\n angle_speed: " << msg.angle_speed + << "\n main_brush_lift: " << msg.main_brush_lift + << "\n edge_brush_lift: " << msg.edge_brush_lift + << "\n vacuum: " << msg.vacuum + << "\n spray: " << msg.spray + << "\n mud_flap: " << msg.mud_flap + << "\n dust_shake: " << msg.dust_shake + << "\n left_light: " << msg.left_light + << "\n right_light: " << msg.right_light + << "\n night_light: " << msg.night_light + << "\n brake_light: " << msg.brake_light + << "\n headlight: " << msg.headlight + << "\n main_brush_spin: " << msg.main_brush_spin + << "\n edge_brush_spin: " << msg.edge_brush_spin + << "\n main_brush_pwm: " << static_cast(msg.main_brush_pwm) + << "\n edge_brush_pwm: " << static_cast(msg.edge_brush_pwm)); } else {