From 92beb100b826e968b3884eaa23087df735c4610c Mon Sep 17 00:00:00 2001 From: lyq Date: Wed, 25 Feb 2026 13:39:57 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=94=B9=E6=8E=A7=E5=88=B6=E9=80=9A?= =?UTF-8?q?=E9=81=93?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../can_radio_ctrl/src/can_radio_ctrl.cpp | 64 +++++++++++-------- src/control/can_radio_ctrl/src/candev.cpp | 18 +++--- 2 files changed, 47 insertions(+), 35 deletions(-) diff --git a/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp b/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp index f58b73c..7200645 100644 --- a/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp +++ b/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp @@ -1,13 +1,13 @@ #include +#include // 补充fabs所需头文件 #include #include -#include // 补充fabs所需头文件 #include "can_radio_ctrl/candev.h" #include "can_radio_ctrl/get_config.h" #include "logger/logger.h" -#include "sweeper_interfaces/msg/mc_ctrl.hpp" #include "sweeper_interfaces/msg/can_frame.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" namespace sweeperMsg = sweeper_interfaces::msg; @@ -32,8 +32,8 @@ class CanRadioCtrlNode : public rclcpp::Node else { LOG_ERROR("Failed to load config, use default values!"); - mcu_rpm_max_ = 1500; // 默认值 - eps_angle_max_ = 45.0f; // 默认值 + mcu_rpm_max_ = 1500; // 默认值 + eps_angle_max_ = 45.0f; // 默认值 } can_Recv_CAN1(); @@ -46,15 +46,13 @@ class CanRadioCtrlNode : public rclcpp::Node "can_data", 10, std::bind(&CanRadioCtrlNode::can_callback, this, std::placeholders::_1)); // 创建周期定时器（每20ms回调一次控制逻辑，50Hz） - timer_ = this->create_wall_timer( - std::chrono::milliseconds(20), - std::bind(&CanRadioCtrlNode::timer_callback, this) - ); + timer_ = + this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&CanRadioCtrlNode::timer_callback, this)); LOG_INFO("CanRadioCtrlNode initialized successfully!"); } - ~CanRadioCtrlNode() + ~CanRadioCtrlNode() { // 停止CAN接收线程 can_Stop_Recv(); @@ -77,12 +75,21 @@ class CanRadioCtrlNode : public rclcpp::Node if (has_received && data_safe) { // 解析挡位 - switch(radio_data.gear) + switch (radio_data.gear) // 通道E { - case 0: msg.gear = 1; break; // R挡 - case 1: msg.gear = 0; break; // N挡 - case 2: msg.gear = 2; break; // D挡 - default: msg.gear = 0; LOG_WARN("Unknown gear: %d", radio_data.gear); break; + case 0: + msg.gear = 1; + break; // R挡 + case 1: + msg.gear = 0; + break; // N挡 + case 2: + msg.gear = 2; + break; // D挡 + default: + msg.gear = 0; + LOG_WARN("Unknown gear: %d", radio_data.gear); + break; } // 解析油门/刹车 @@ -100,7 +107,7 @@ class CanRadioCtrlNode : public rclcpp::Node } // 解析清扫开关 - msg.sweep = (radio_data.sweep != 0); + msg.sweep = (radio_data.sweep != 0); // 通道B // 解析转向 float steering_ratio = static_cast(radio_data.steering) / 450.0f; @@ -114,9 +121,7 @@ class CanRadioCtrlNode : public rclcpp::Node float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO; // 限制电机转速到 [120, 1500] 范围 - uint16_t can_speed = static_cast( - std::clamp(motor_rpm, 120.0f, 1500.0f) - ); + uint16_t can_speed = static_cast(std::clamp(motor_rpm, 120.0f, 1500.0f)); msg.angle = target_angle; msg.angle_speed = can_speed; @@ -129,11 +134,17 @@ class CanRadioCtrlNode : public rclcpp::Node { const char* brake_str = msg.brake ? "已刹车" : "未刹车"; const char* gear_str = "未知挡位"; - switch(msg.gear) + switch (msg.gear) { - case 0: gear_str = "空挡"; break; - case 1: gear_str = "后退挡"; break; - case 2: gear_str = "前进挡"; break; + case 0: + gear_str = "空挡"; + break; + case 1: + gear_str = "后退挡"; + break; + case 2: + gear_str = "前进挡"; + break; } const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫"; @@ -145,8 +156,7 @@ class CanRadioCtrlNode : public rclcpp::Node " 轮端转向角度: %.1f °\n" " 清扫状态: %s\n" "=====================================", - brake_str, gear_str, static_cast(msg.rpm), msg.angle, sweep_str - ); + brake_str, gear_str, static_cast(msg.rpm), msg.angle, sweep_str); print_counter = 0; // 重置计数器 } } @@ -175,7 +185,7 @@ class CanRadioCtrlNode : public rclcpp::Node { // 增加空指针检查，避免崩溃 if (!msg) return; - + // 判断是否为转向反馈帧（ID=0x401，且数据长度大于5） if (msg->id == 0x401 && msg->dlc >= 5) { @@ -196,8 +206,8 @@ class CanRadioCtrlNode : public rclcpp::Node float current_feedback_angle; RmoCtlConfig config; int print_counter; - int mcu_rpm_max_; // MCU最大转速 - float eps_angle_max_;// EPS最大角度 + int mcu_rpm_max_; // MCU最大转速 + float eps_angle_max_; // EPS最大角度 }; int main(int argc, char* argv[]) diff --git a/src/control/can_radio_ctrl/src/candev.cpp b/src/control/can_radio_ctrl/src/candev.cpp index 4f40417..ccaf132 100644 --- a/src/control/can_radio_ctrl/src/candev.cpp +++ b/src/control/can_radio_ctrl/src/candev.cpp @@ -116,16 +116,18 @@ void can_recv_loop() } else if (frame.can_id == 0x10A && frame.can_dlc >= 8) { - radio_data.gear = frame.data[0]; - radio_data.sweep = frame.data[1]; - uint8_t ch3 = frame.data[2]; - uint8_t ch4 = frame.data[3]; - uint8_t ch5 = frame.data[4]; - uint8_t ch6 = frame.data[5]; + uint8_t ch1 = frame.data[0]; // 通道E + uint8_t ch2 = frame.data[1]; // 通道F + uint8_t ch3 = frame.data[2]; // 通道A + uint8_t ch4 = frame.data[3]; // 通道B + uint8_t ch5 = frame.data[4]; // 通道C + uint8_t ch6 = frame.data[5]; // 通道D uint8_t ch7 = frame.data[6]; uint8_t ch8 = frame.data[7]; - printf("ch1 : %d ,ch2 : %d ,ch3 : %d ,ch4 : %d\n", radio_data.gear, radio_data.sweep, ch3, ch4); - printf("ch5 : %d ,ch6 : %d ,ch7 : %d ,ch8 : %d\n", ch5, ch6, ch7, ch8); + radio_data.gear = ch1; + radio_data.sweep = ch4; + // printf("ch1 : %d ,ch2 : %d ,ch3 : %d ,ch4 : %d\n", ch1, ch2, ch3, ch4); + // printf("ch5 : %d ,ch6 : %d ,ch7 : %d ,ch8 : %d\n", ch5, ch6, ch7, ch8); radio_data.data_valid = true; } }