lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更改控制通道

Browse Source
This commit is contained in:
lyq 2026-02-25 13:39:57 +08:00
parent 09fa75aa90
commit 92beb100b8
2 changed files with 47 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/control/can_radio_ctrl/src/can_radio_ctrl.cpp
View File

@ -1,13 +1,13 @@
#include <algorithm>
#include <cmath> // 补充fabs所需头文件
#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <cmath> // 补充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<float>(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<uint16_t>(
std::clamp(motor_rpm, 120.0f, 1500.0f)
);
uint16_t can_speed = static_cast<uint16_t>(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<int>(msg.rpm), msg.angle, sweep_str
);
brake_str, gear_str, static_cast<int>(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[])

18
src/control/can_radio_ctrl/src/candev.cpp
View File

@ -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;
}
}